;Schrittmotor-Steuerung für ISEL-Karte MPK3
;UseVTD equ 1 ;Timer statt CMOS benutzen
;UseLoop equ 1 ;Schleife statt Summenformel benutzen
;LogInt equ 1 ;Jeden Interrupt loggen
.386 ;32bit-Segmente aktivieren
include tvmm.inc ;Generelles
include debug.inc ;wegen Debugging
IFDEF UseVTD
include vtd.inc ;Zeitgeber-Services
ELSE
include vpicd.inc ;Interrupt-Controller
ENDIF
MPK3D_Device_ID equ 378Ch ;von Microsoft bestätigt:
;>You have been assigned a device ID of 378Ch for your MPK3D.386 virtual
;>device. This number is uniquely assigned to this device.
MPK3D_Major_Ver equ 1
MPK3D_Minor_Ver equ 0
;=============================
bit equ <1 shl>
crlf equ <13,10>
;=============================
bres macro r,b:rest
maskflag r,not (b)
endm
;=============================
bset macro r,b:rest
setflag r,b
endm
;=============================
btst macro r,b:rest
testflag r,b
endm
;=============================
dz macro str:rest
db str,0
endm
;=============================
ld macro r1,r2
push r2
pop r1
endm
;=============================
Debug_Halt macro
ifdef DEBUG
int 1
endif
endm
;=============================
MIN macro r,c
local l1
cmp r,c
jc short l1
mov r,c
l1:
endm
;=============================
MAX macro r,c
local l1
cmp r,c
jnc short l1
mov r,c
l1:
endm
;=============================
RABS macro r
local l1
or r,r
jns short l1
neg r
l1:
endm
;=============================
ifdef DEBUG
%OUT !Assembliere Debugversion!
endif
BeginInterrupts macro
IFDEF UseVTD
MASM
mov eax,1
VxDcall VTD_Begin_Min_Int_Period
mov esi,OFFSET32 intproc
VMMCall Set_Global_Time_Out
IDEAL
ENDIF
endm
EndInterrupts macro
IFDEF UseVTD
MASM
mov eax,1
VxDcall VTD_End_Min_Int_Period
IDEAL
ENDIF
endm
locals @@
Declare_Virtual_Device MPK3D, MPK3D_Major_Ver, MPK3D_Minor_Ver,\
MPK3D_Control_Proc, MPK3D_Device_ID,\
Undefined_Init_Order,MPK3D_API,MPK3D_API
;*********************************************************
;*** Obligatorischer VxD-Botschafts-Funktionsverteiler ***
;*********************************************************
VxD_Locked_Code_Seg
BeginProc MPK3D_Control_Proc
Control_Dispatch Sys_Critical_Init, MPK3D_Init
Control_Dispatch System_Exit, MPK3D_Exit
clc
ret
EndProc MPK3D_Control_Proc
VxD_Locked_Code_ends
;********************************************************
;*** Residenter Datenbereich und Konstantendefinition ***
;********************************************************
VxD_Locked_Data_Seg
IDEAL
;Folgende Kommandos gibt es:
SM_Nop equ 0 ;"NOP"-Kommando
SM_Free equ 1 ;Motorspulen stromfrei schalten
SM_Sync equ 2 ;Referenzfahrt, EDX=Referenzpunkt
SM_MoveAbs equ 3 ;Absolutbewegung
SM_MoveRel equ 4 ;Relativbewegung
SM_Assign equ 5 ;Belegung, Motor stromfrei
SM_UnAssign equ 6 ;Freigabe, Motor stromfrei
SM_SetSA equ 7 ;Geschwindigkeit und Beschl. festlegen **)
SM_GetSA equ 8 ;Geschw. und Beschleunigung erfragen
SM_SetPostMsg equ 9 ;PostMessage-Adresse und Fenster festlegen **)
SM_SetPostMsg equ 10 ;obiges erfragen
SM_GetPosition equ 11 ;Position und Geschw. erfragen (EDX u. ECX)
SM_SetBounds equ 12 ;Begrenzungen setzen **)
SM_GetBounds equ 13 ;Begrenzungen erfragen
SM_Stop equ 14 ;Soforthalt
SM_SetGear equ 15 ;Übersetzungsverhältnis setzen **)
SM_GetGear equ 16 ;Übersetzungsverhältnis erfragen
SM_SetHWE equ 17 ;Hardware-Endschalter-Maske setzen
;**) EDX und ECX enthalten bei Rückkehr die _alten_ Werte
SM_LastFunc equ 17
;{Fehlercodes von Isel}
ME_NoError equ 0;
ME_WrongFunction equ 1;
ME_InMove equ 4;
ME_SoftEnd equ 5;
ME_HardEnd equ 7;
ME_NoRef equ 9;
;{Eigene Fehlercodes}
ME_NotAssigned equ 50;
ME_AlreadyAssigned equ 51;
ME_NoDriver equ 52;
INVPOS equ 80000000h ;Ungültige Position = -MaxLong-1
IFDEF UseVTD
FREQ equ 1000
ELSE
IrqHand dd ? ;IRQ-Griff
FREQ equ 1024
ENDIF
Frequency dd FREQ ;für Zeitmessungen usw.
upcounter dd ?
struc TMotor
Flags dw 0
HWEMask db ?
HWEXor db 0
PortA db ?
PortB db ?
HWPos dw 0
Pos dd INVPOS ;bedeutet "ungültige Position"
union
struc
WegLo dd 0 ;(Rest-)Weg in Nanoschritten (2**-16 Mikroschr.)
WegHi dd 0 ;(Rest-)Weg
ends
struc
fill1 dw ?
WegMid dd ?
fill2 dw ?
ends
ends
Refpoint dd 0 ;Referenzpunkt nach Sync
Speed dd 0 ;Momentangeschwindigkeit
MaxSpeed dd 48*65536;Maximalgeschwindigkeit
RefSpeed dd 12*65536;Referenz-Geschwindigkeit
MaxAccel dd 48 ;Maximalbeschleunigung
LeftBound dd -7FFFFFFFh ;Grenzen enthalten Anwenderkoordinaten!
RightBound dd 7FFFFFFFh
Umrech1 dd 1
Umrech2 dd 1
CallbackAddr dd 0
CallbackWnd dd 0
ends TMotor
MotDef dd 48*65536,12*65536,48,-7FFFFFFFh,7FFFFFFFh,1,1
;Defaults ab MaxSpeed
MotDefElems equ 7 ;7 Elemente initialisieren (Länge obiges DD)
PMotor equ (TMotor ebx) ;Objektzeiger ist immer EBX
MotX TMotor <0,4,4,0,1>
MotY TMotor <0,2,2,2,3>
MotZ TMotor <0,1,1,4,5>
;Mit den Bits scheint irgendetwas verdreht zu sein!
PortBase dw 340h ;Port-Basisadresse
;Die Adresse 347h wird durch das VxD zu einem rücklesbaren Ausgabeport
;umfunktioniert, welches die Leistungsendstufe (Bit0) und das Relais
;(Bit1) steuert. Das Port 346h bleibt in seiner Funktionalität; Schreib-
;zugriffe wirken genauso wie auf 347h - verändern also "seinen" Spiegel.
;Die Motor-Phasenstrom-Ports sind einfach nur rücklesbar.
;Da das Gesamtverhalten dem Original entspricht, sollte auch die
;Originalsoftware laufen; allerdings werden die Portzugriffe zusätzlich
;durch das Trapping verlangsamt.
PortMirror db 7 dup (?) ;7 Ausgabeport-Spiegel
;Sinus-Tabelle OHNE 0- und 1-Punkt, zentriert auf die Mitte zwischen 7F und 80
;Bedingt durch diese seltsame Zentrierung in der Hardware gibt es keinen
;100% stromfreien Zustand der Motorspule.
;Diese Sinustabelle enthält 256 Werte im Bereich 128..255 für den
;1. Quadranten. Die Quadrantenumrechnung erfolgt durch das Programm GetCos
;bzw. GetSin.
SinTab:
db 128,129,130,131,132,132,133,134,135,135,136,137,138,139,139,140
db 141,142,142,143,144,145,145,146,147,148,149,149,150,151,152,152
db 153,154,155,155,156,157,158,158,159,160,161,161,162,163,164,164
db 165,166,167,167,168,169,170,170,171,172,173,173,174,175,176,176
db 177,178,178,179,180,181,181,182,183,183,184,185,185,186,187,188
db 188,189,190,190,191,192,192,193,194,194,195,196,196,197,198,198
db 199,200,200,201,201,202,203,203,204,205,205,206,206,207,208,208
db 209,209,210,211,211,212,212,213,214,214,215,215,216,216,217,218
db 218,219,219,220,220,221,221,222,222,223,223,224,224,225,225,226
db 226,227,227,228,228,229,229,230,230,231,231,232,232,233,233,233
db 234,234,235,235,236,236,236,237,237,238,238,238,239,239,239,240
db 240,241,241,241,242,242,242,243,243,243,244,244,244,245,245,245
db 245,246,246,246,247,247,247,247,248,248,248,248,249,249,249,249
db 250,250,250,250,251,251,251,251,251,251,252,252,252,252,252,252
db 253,253,253,253,253,253,253,254,254,254,254,254,254,254,254,254
db 254,254,255,255,255,255,255,255,255,255,255,255,255,255,255,255
MASM
VxD_Locked_Data_Ends
;*****************************
;*** Sinustabellen-Zugriff ***
;*****************************
VxD_Locked_Code_Seg
IDEAL
;Wegen der besonderen Zentrierung der Sinustabelle erfolgt die Quadranten-
;umrechnung hier nicht mit dem sonst üblichen NEG (Zweierkomplement),
;sondern mit NOT (Einerkomplement).
;Bei gesetztem Dreieck-Bit wird das Ergebnis der Dreiecksfunktion geliefert.
proc GetCos ;PE: AX: Winkelargument, Vollwinkel=1024
;PA: AL: 7F-80h-zentriertes Funktionsergebnis
;VR: AX
inc ah
GetSin:
BTST [PMotor.Flags],bit 5 ;Sinus/Dreieck-Schalter
jnz @@dreieck
push ebx
lea ebx,[SinTab]
test ah,bit 0
jz @@1 ;1. oder 3. Quadrant
not al ;Bit-Komplement bilden
@@1: xlat ;Tabellenzugriff
test ah,bit 1;3. oder 4. Quadrant?
jz @@2 ;1. oder 2. Quadrant (bleibt >=80h)
not al ;Bit-Komplement bilden (negative Werte <80h)
@@2: pop ebx
ret
@@dreieck:
test ah,bit 0 ;2./4. Quadrant?
jz @@3 ;nein
not al ;ja, 00-->FF, FF-->00
@@3: stc ;0-->80, 1-->80, 2-->81, 3-->81, 4-->82,...
rcr al,1 ;FD-->FE, FE-->FF, FF-->FF (Erster Quadrant)
test ah,bit 1 ;3./4. Quadrant
jz @@4 ;nein, positiv lassen
not al ;ja, 80-->7F, 81-->7E,... FF-->00
@@4: ret
endp GetCos
;*************************************************
;*** Portzugriffe auf CMOS und ISEL-MPK3-Karte ***
;*************************************************
IFNDEF UseVTD
proc GetCMOS
;PE: AH=Adresse im CMOS-RAM
;PA: AL=Wert
;VR: AL
pushf
cli
xchg al,ah
out 70h,al
IO_DELAY
xchg al,ah
in al,71h
popf
ret
endp GetCMOS
proc PutCMOS
;PE: AH=Adresse im CMOS-RAM
; AL=Wert
;VR: -
pushf
cli
xchg al,ah
out 70h,al
IO_DELAY
xchg al,ah
out 71h,al
popf
ret
endp PutCMOS
ENDIF
;Ausgabe eines Bytes mit Merkfunktion im "Spiegel"
;PE: AH=Index (0..6), AL=auszugebendes Byte
; Ist AH>6 wird es auf 6 gesetzt
;PA: -
;VR: ggf. AH
;KO: Ein kurzes CLI sorgt für garantierte Gleichheit des Spiegels
; mit dem ausgegebenen Wert in allen (INT-)Lebenslagen
proc OutB
push edx
pushfd
MIN ah,6
movzx edx,ah
cli
mov [PortMirror+EDX],al
add dx,[PortBase]
out dx,al
popfd
pop edx
ret
endp
;Einlesen eines Bytes vom Spiegelport oder "echt", wenn AH=6
;PE: AH=Index (0..7) Ist AH>6 wird der Spiegel von PortBase+6 gelesen
;PA: AL=eingelesenes Byte
;VR: AL, ggf. AH, Flags
proc InB
push edx
MIN ah,6
movzx edx,ah
mov al,[PortMirror+EDX]
jnz @@e ;wenn AH<> 6 war!
add dx,[PortBase] ;wenn AH=6 direkt einlesen
in al,dx
@@e: pop edx
ret
endp
;*******************************
;*** Direkte Motor-Steuerung ***
;*******************************
;Ausschalten des Stroms durch die Spulen
;PE: EBX: Objektzeiger
;VR: AX
proc motor_aus
mov ah,[PMotor.PortA]
mov al,128
call OutB
mov ah,[PMotor.PortB]
call OutB ;Spulen (fast) stromlos machen
mov [PMotor.Pos],InvPos ;Unbestimmte Position setzen
ret
endp motor_aus
;Allokieren des Motors; Spulenstrom sicherheitshalber ausschalten,
;jedoch Leistungsendstufen EINschalten
;PE: EBX: Objektzeiger
;VR: AX
proc motor_init
mov [PMotor.Flags],bit 0 ;in Beschlag nehmen, andere Bits AUS!
call motor_aus
lea esi,[MotDef]
lea edi,[PMotor.MaxSpeed]
mov ecx,MotDefElems
cld
rep movsd
mov ah,7
call InB
BSET al,bit 0
call OutB ;Spulen einschalten
IFNDEF UseVTD
mov ah,0bh
call GetCMOS
BSET al,bit 6 ;CMOS Interrupt 1024Hz aktivieren
call PutCMOS
mov eax,[IrqHand]
VxDcall VPICD_Phys_EOI ;evtl. verklemmten PIC befreien
ENDIF
ret
endp motor_init
;Freigeben des Motors. Spulenstrom wird ausgeschaltet.
;Sind alle Motoren frei, werden die Leistungsendstufen abgeschaltet
;PE: EBX: Objektzeiger
;VR: AX
proc motor_done
call motor_aus
BRES [PMotor.Flags],bit 0
BTST [MotX.Flags],bit 0 ;In Beschlag?
jnz @@e
BTST [MotY.Flags],bit 0
jnz @@e
BTST [MotZ.Flags],bit 0
jnz @@e
mov ah,7
call InB
BRES al,bit 0 ;Alles AUSmachen! (außer Relais)
call OutB
@@e:
ret
endp motor_done
;Motor um vzb. Betrag bewegen, dieser sollte -256..256 nicht überschreiten.
;Dabei wird die Positions-Arbeitszelle mitgeführt, sofern sie nicht
;den Wert "InvPos" enthielt (d.h. die aktuelle Position unbestimmt ist)
;PE: EAX: vzb. Betrag (möglichst -256..256)
; EBX: Objektzeiger
;VR: AX
proc motor_move
cmp [PMotor.Pos],InvPos
jz @@1
add [PMotor.Pos],eax
@@1:
add [PMotor.HWPos],ax
mov ax,[PMotor.HWPos]
push ax
call GetCos
mov ah,[PMotor.PortA]
call OutB
pop ax
call GetSin
mov ah,[PMotor.PortB]
call OutB
ret
endp motor_move
;Hardware-Endschalter abfragen
;PE: EBX=Objektzeiger
;PA: NZ: Schalter betätigt; AL enthält die gesetzten Bits
;VR: AX
proc TestSchalter
mov ah,6
call InB
xor al,[PMotor.HWEXor]
and al,[PMotor.HWEMask]
ret
endp TestSchalter
;****************************
;*** Periodische Aktionen ***
;****************************
proc intreffahrt
IFDEF LogInt
MASM
Debug_Out "IntSync "
IDEAL
ENDIF
call TestSchalter ;Hardware-Endschalter abfragen
jnz @@1
xor eax,eax
BTS [PMotor.Flags],3 ;Merkbit "Nicht auf Endschalter"
jnc @@3 ;im Wendepunkt 1x aussetzen
BT [PMotor.Flags],7
@@5: mov eax,[PMotor.RefSpeed]
@@3: call MoveBySpeedDir
ret
@@1:
BTST [PMotor.Flags],bit 3 ;Gerade draufgekommen?
jnz @@4
BT [PMotor.Flags],7 ;nein, etwas zurückfahren!
cmc
jmp @@5
@@4:
mov eax,[PMotor.Refpoint]
SetRefposHere:
mov [PMotor.Pos],eax
xor eax,eax
mov [PMotor.HWPos],ax
call motor_move ;auf eine Null-Position festlegen
BRES [PMotor.Flags],0Eh ;Bit1+Bit2+Bit3
EndInterrupts
call InformWindows
ret
endp intreffahrt
proc intmove
BTST [PMotor.Flags],bit 1
jz @@e
BTST [PMotor.Flags],bit 2
jz @@m
call intreffahrt
jmp @@e
@@m: call intmoveproc
@@e: ret
endp
proc intproc ;Das Innere des Timer-Interrupts
IFDEF UseVTD
mov eax,1
mov esi,OFFSET32 intproc
MASM
VMMcall Set_Global_Time_Out
IDEAL
ENDIF
lea ebx,[MotX]
call intmove
lea ebx,[MotY]
call intmove
lea ebx,[MotZ]
call intmove
inc [upcounter]
ret
endp intproc
;*****************************************
;*** Aktions-Anstöße für Sync und Move ***
;*****************************************
;PE: EAX=vzb. Differenz
proc StartSegment
BRES [PMotor.Flags],bit 7 ;Positiv annehmen
sub eax,[PMotor.Pos] ;Differenz
jge @@1
neg eax ;Betrag bilden
BSET [PMotor.Flags],bit 7 ;und Richtung merken
@@1: push eax
xor eax,eax
mov [PMotor.Speed],eax ;Geschwindigkeit nullsetzen
mov [PMotor.WegLo],eax
mov [PMotor.WegHi],eax
pop eax
mov [PMotor.WegMid],eax ;und Weg entsprechend setzen
BeginInterrupts
BSET [PMotor.Flags],Bit 1 ;Und LOS!
@@e: ret
endp
proc StartReffahrt
cmp eax,InvPos ;INVPOS: Setzen Referenz HIER!
jz @@a ;(zum Debuggen ohne Hardware)
mov [PMotor.Refpoint],eax ;wird dann eingetragen
mov [PMotor.Speed],0
BRES [PMotor.Flags],bit 3 ;Annahme: Motor steht AUF Endschalter
BeginInterrupts
BSET [PMotor.Flags],bit 1 + bit 2 ;ISR werkeln lassen
@@e: ret
@@a: xor eax,eax
jmp SetRefposHere ;Debugging-Referenzpunkt
endp
;*********************************************
;*** Testfunktionen mit Fehlercoderückgabe ***
;*********************************************
;Testet, ob Motor belegt, und liefert, wenn ja, CY=1 und AL=Code zurück
proc TestFrei
BTST [PMotor.Flags],bit 0 ;Belegt?
jz @@e ;nein, okay
mov al,ME_AlreadyAssigned
stc
@@e: ret
endp
;Testet, ob Motor belegt, und liefert, wenn nicht, CY=1 und AL=Code zurück
proc TestBelegt
BTST [PMotor.Flags],bit 0 ;Belegt?
jnz @@e ;ja, okay
mov al,ME_NotAssigned
stc
@@e: ret
endp
;Testet, ob Motor in Bewegung ist, und liefert, wenn ja, CY=1 und AL=Code zurück
proc TestStill
BTST [PMotor.Flags],bit 1 ;In Bewegung?
jz @@e ;nein, okay
mov al,ME_InMove
stc
@@e: ret
endp
;Testet, ob Motor gültige Position hat, d.h. ob eine Referenzfahrt
;ausgeführt wurde
proc TestReferenz
cmp [PMotor.Pos],InvPos ;Ungültig?
clc
jnz @@e ;Gültig!
mov al,ME_NoRef
stc
@@e: ret
endp
proc TestInBounds
cmp eax,[PMotor.LeftBound]
jl @@e
cmp eax,[PMotor.RightBound]
jg @@e
clc
ret
@@e: mov al,ME_SoftEnd
stc
ret
endp
;******************************************
;*** Rückruf-Funktionen für PostMessage ***
;******************************************
proc PostEventProc
mov ebx,edx ;geordnete Verhältnisse
cmp [PMotor.CallbackAddr],0
jz @@e
Debug_Halt
MASM
Push_Client_State
VMMCall Begin_Nest_Exec
IDEAL
mov ax,[word LOW PMotor.CallbackWnd] ;Window
MASM
VMMCall Simulate_Push
IDEAL
mov ax,[word HIGH PMotor.CallbackWnd] ;Message
MASM
VMMCall Simulate_Push
xor eax,eax ;wParam=0
VMMCall Simulate_Push
IDEAL
mov eax,[PMotor.Pos] ;lParam=Position
call HerUeber ;Wandeln!
push eax
shr eax,16 ;erst High-Teil (Umm!)
MASM
VMMCall Simulate_Push
IDEAL
pop eax ;dann Low-Teil!
MASM
VMMCall Simulate_Push
IDEAL
movzx edx,[word LOW PMotor.CallbackAddr]
mov cx,[word HIGH PMotor.CallbackAddr]
MASM
VMMCall Simulate_Far_Call
VMMCall Resume_Exec
VMMCall End_Nest_Exec
Pop_Client_State
IDEAL
@@e:
ret
endp
proc InformWindows c
uses ebx,ecx,edx
mov edx,ebx
mov eax,High_Pri_Device_Boost
MASM
VMMCall Get_Sys_VM_Handle
mov ecx,PEF_Wait_For_STI or PEF_Wait_Not_Crit or PEF_Always_Sched
lea esi,[PostEventProc]
xor edi,edi ;Timeout sicherheitshalber auf Null
VMMCall Call_Priority_VM_Event
IDEAL
ret
endp
;***********************************************
;*** Berechnung des Geschwindigkeitsverlaufs ***
;***********************************************
;PE: EAX=vzl. Geschwindigkeit in Nanoschritten
; EBX=Objektzeiger
;bei MoveBySpeedDir: CY=1: Rückwärts
;PA: - Die Momentangeschwindigkeit wird auf EAX gesetzt
; - Der Rest-Weg wird entsprechend "gekürzt"
; - Entsprechend der sich daraus ergebenden Nanoschritt-Differenz
; werden Mikroschritte in die in Flags angegebene Richtung ausgeführt
proc MoveBySpeed
BT [PMotor.Flags],7 ;Richtungs-Bit nach CY
MoveBySpeedDir:
pushfd
mov [PMotor.Speed],eax
push edx
mov edx,[PMotor.WegMid]
sub [PMotor.WegLo],eax
sbb [PMotor.WegHi],0 ;Restweg verringert sich
sub edx,[PMotor.WegMid] ;Differenz!
mov eax,edx
pop edx
popfd
jnc @@p
neg eax
@@p: call motor_move
ret
endp
;Funktion "Bremsweg berechnen"
;PE: EAX: Momentane (angenommene) Geschwindigkeit, vzl., in Nanoschritten
; EBX: Zeiger auf Bewegungsstruktur (mit vzl. MaxAccel)
;PA: EDX:EAX: Bremsweg (vzl.)
;VR: EDX,EAX
IFDEF UseLoop
proc Bremsweg c
uses esi
;for(EDX:ESI=0, EAX=Speed; EAX>0; EAX-=MaxAccel) EDX:ESI+=ECX;
xor edx,edx
xor esi,esi
@@l: add ecx,eax
adc edx,0
sub eax,[PMotor.MaxAccel]
ja @@l
mov eax,esi
ret
endp
ELSE
;Variante 2: schleifenlos
proc Bremsweg c
uses esi,edi
push eax ;A1
xor edx,edx
div [PMotor.MaxAccel]
inc eax ;N ermitteln = A1/d +1
push eax
mov edx,eax
dec edx
mul edx ;N(N+1)
shr eax,1 ;N(N+1)/2
mul [PMotor.MaxAccel] ;EDX:EAX=N(N-1)d/2
mov esi,edx
mov edi,eax ;nach ESI:EDI
pop eax ;N
pop edx ;A1
mul edx
sub eax,edi
sbb edx,esi
ret
endp
ENDIF
;Funktion "Geschwindigkeit berechnen"
;Anhand des noch zurückzulegenden Restwegs (PMotor.Weg) und
;weiterer Parameter wird ein neuer - möglichst maximaler -
;Geschwindigkeitswert ermittelt
;PE: EBX: Zeiger auf Bewegungsstruktur
;PA: EAX: Neue Geschwindigkeit
proc CalcNewSpeed c
uses ecx,edx,esi
mov eax,[PMotor.Speed]
call Bremsweg ;Daraus Bremsweg ermitteln
stc
sbb eax,[PMotor.WegLo] ;(Bremsweg-=Restweg+1)
sbb edx,[PMotor.WegHi] ;Zu lang?
jnc @@ReduceSpeed ;wenn Bremsweg>Restweg
mov eax,[PMotor.Speed]
cmp [PMotor.MaxSpeed],eax ;Zu groß, wenn Höchstgeschwindigkeit
jc @@ReduceSpeed ;verringert wurde
add eax,[PMotor.MaxAccel] ;Beschleunigen (versuchen)
MIN eax,[PMotor.MaxSpeed] ;Höchstgeschwindigkeit!
mov esi,eax ;Merken
cmp [PMotor.WegHi],0
jnz @@1
MIN esi,[PMotor.WegLo] ;Extremfall: Sehr kurzer Weg<Accel
@@1: call Bremsweg
stc
sbb eax,[PMotor.WegLo] ;Bremsweg wäre zu lang?
sbb edx,[PMotor.WegHi]
jnc @@NoAccel
mov eax,esi ;Beschleunigungs- oder Fahrphase
jmp @@e
@@NoAccel: ;(Kurze) Haltephase beim Abbremsen
mov eax,[PMotor.Speed]
jmp @@e
@@ReduceSpeed:
mov eax,[PMotor.Speed]
sub eax,[PMotor.MaxAccel]
ja @@e ;Stets >0
mov eax,[PMotor.WegLo]
@@e:
ret
endp CalcNewSpeed
proc intmoveproc
IFDEF LogInt
MASM
Debug_Out "IntMove "
IDEAL
ENDIF
mov eax,[PMotor.WegLo]
or eax,[PMotor.WegHi] ;Nichts mehr tun?
jz @@1
BTST [PMotor.Flags],bit 4 ;HWE-Schaltertest?
jz @@2
call TestSchalter ;Hardware-Endschalter abfragen
jnz @@3
@@2: call CalcNewSpeed
call MoveBySpeed
ret
@@3: xor eax,eax
mov [PMotor.WegLo],eax
mov [PMotor.WegHi],eax
mov [PMotor.Pos],InvPos
@@1:
mov [PMotor.Speed],eax
BRES [PMotor.Flags],bit 1 ;Bewegungssegment ENDE
EndInterrupts
call InformWindows
ret
endp
;************************************************************
;*** Übersetzung zwischen System- und Anwenderkoordinaten ***
;************************************************************
;Hin-Übersetzung: Schritt-Postition:=User-Position*Umrech2/Umrech1
;PE: EAX=Anwender-Position
; EBX=Objektzeiger
;PA: EAX=Schrittmotor-Position, CY=0
;VR: EAX, Flags
;N: Die "ungültige Position" INVPOS wird niemals umgerechnet
proc HinUeber c
uses edx
cmp eax,InvPos
jz @@e
imul [PMotor.Umrech2]
idiv [PMotor.Umrech1]
; shl edx,1 ;Rest verdoppeln
; cmp [PMotor.Umrech1],edx
; adc eax,0 ;Runden (so richtig NUR wenn Umrech1 positiv!)
@@e: clc
ret
endp
HerUeberA: ;EAX=vzl.Beschleunigung in Nanoschritt/Tick pro Tick
push edx
mul [Frequency]
pop edx
HerUeberV: ;EAX=vzl.Geschwindigkeit in Nanoschritt pro Tick
push edx
mul [Frequency]
shrd eax,edx,16
pop edx
;Her-Übersetzung: User-Postition:=Schritt-Position*Umrech1/Umrech2
;PE: EAX=Schrittmotor-Position
; EBX=Objektzeiger
;PA: EAX=Anwender-Position, CY=0
;VR: EAX, Flags
;N: Die "ungültige Position" INVPOS wird niemals umgerechnet
proc HerUeber c
uses edx
cmp eax,InvPos
jz @@e
imul [PMotor.Umrech1]
idiv [PMotor.Umrech2]
; shl edx,1 ;Rest verdoppeln
; cmp [PMotor.Umrech2],edx
; adc eax,0 ;Runden (so richtig NUR wenn Umrech2 positiv!)
@@e: clc
ret
endp
HinUeberV:
mov cl,1
;Hinüber-Übersetzung mit nachfolgender Betragsbildung.
;Ist das Ergebnis Null, wird 1 gesetzt
;PE: CL=1 (v) oder 2 (a)
;VR: CL, EAX
proc HinPositiv c
uses edx
call HinUeber
mov edx,eax
sar edx,16
shl eax,16 ;EDX:EAX:=EAX*65536, vzb.
@@l: idiv [Frequency]
cdq ;Rest verwerfen
dec cl
jnz @@l
RABS eax ;Betrag bilden
jnz @@2 ;Null? nein
inc eax ;ja: EAX:=1
@@2: ret
endp
;***************************
;*** Neue API-Funktionen ***
;***************************
proc SMNop ;"NOP"-Kommando
ret
endp
;*************************************
proc SMFree ;Motorspulen stromfrei schalten
call TestBelegt
jc @@e
call TestStill
jc @@e
push eax
call motor_aus
pop eax
@@e:
ret
endp
;*************************************
proc SMSync ;Referenzfahrt
call TestBelegt
jc @@e
call TestStill
jc @@e
cmp eax,InvPos
jz @@1 ;"Debug-Position"
call TestInBounds
jc @@e
call HinUeber
xchg edx,eax
BRES [PMotor.Flags],bit 7
or eax,eax
jns @@p
BSET [PMotor.Flags],bit 7
neg eax
@@p: jz @@q ;Keine Geschwindikeit setzen
cmp eax,1 ;Betrag war 1?
jz @@q ;dann auch keine Geschwindigkeit setzen
call HinUeberV
mov [PMotor.RefSpeed],eax
@@q: xchg edx,eax
@@1: call StartReffahrt
@@e:
ret
endp
;*************************************
proc SMMoveAbs ;Absolutbewegung
call TestBelegt
jc @@e
call TestStill
jc @@e
call TestReferenz
jc @@e
call TestInBounds
jc @@e
call HinUeber
call StartSegment
@@e:
ret
endp
;*************************************
proc SMMoveRel ;Relativbewegung
call TestBelegt
jc @@e
call TestStill
jc @@e
call TestReferenz
jc @@e
call TestInBounds
jc @@e
call HinUeber
add eax,[PMotor.Pos]
call StartSegment
@@e:
ret
endp
;*************************************
proc SMAssign ;Belegung
call TestFrei
jc @@e
call motor_init
@@e:
ret
endp
;*************************************
proc SMUnAssign ;Freigabe
call SMFree
jc @@e
call motor_done
mov [PMotor.CallbackAddr],0 ;Nie mehr rückrufen!
@@e:
ret
endp
;*************************************
proc SMSetSA ;Geschwindigkeit und Beschleunigung festlegen
call TestBelegt
jc @@e
call HinUeberV ;Geschwindigkeit hinrechnen
xchg [PMotor.MaxSpeed],eax
call HerUeberV ;Geschwindigkeit rückrechnen
RABS eax
xchg eax,edx
mov cl,2
call HinPositiv ;Beschleunigung hinrechnen
xchg [PMotor.MaxAccel],eax
call HerUeberA ;Beschleunigung rückrechnen
RABS eax
xchg edx,eax
IFDEF UseLoop
push edx eax
mov eax,[PMotor.MaxSpeed]
xor edx,edx
div [Frequency]
MAX [PMotor.MaxAccel],eax ;Beschleunigung mindestens,
;daß max. 1 Sekunde Beschleunigungsphase ist
pop eax edx
ENDIF
@@e:
ret
endp
;*************************************
proc SMGetSA ;Geschwindigkeit und Beschleunigung erfragen
call TestBelegt
jc @@e
mov eax,[PMotor.MaxAccel]
call HerUeberA
RABS eax
xchg edx,eax
mov eax,[PMotor.MaxSpeed]
call HerUeberV
RABS eax
@@e:
ret
endp
;*************************************
proc SMSetCallback ;Rückrufadresse (@PostMessage) sowie
;Fenster und WM-Nachricht festlegen
;Diese wird ausschließlich in die System-VM gerufen!
;Window=angegeben, Message=angegeben, wParam=0 (Ursache), lParam=Position
call TestBelegt
jc @@e
xchg [PMotor.CallbackAddr],eax
xchg [PMotor.CallbackWnd],edx
@@e:
ret
endp
;*************************************
proc SMGetCallback ;Obiges zurücklesen
call TestBelegt
jc @@e
mov eax,[PMotor.CallbackAddr]
mov edx,[PMotor.CallbackWnd]
@@e:
ret
endp
;*************************************
proc SMGetPosition ;liefert Position und Geschwindigkeit
call TestBelegt
jc @@e
pushf
cli ;Ein zusammenhängendes Paar erwischen!
mov eax,[PMotor.Pos]
mov edx,[PMotor.Speed]
popf
call HerUeber
xchg eax,edx
call HerUeber
xchg edx,eax
@@e:
ret
endp
;*************************************
proc SMSetBounds
call TestBelegt
jc @@e
xchg [PMotor.LeftBound],eax
xchg [PMotor.RightBound],edx
@@e:
ret
endp
;*************************************
proc SMGetBounds
call TestBelegt
jc @@e
mov eax,[PMotor.LeftBound]
mov edx,[PMotor.RightBound]
@@e:
ret
endp
;*************************************
proc SMStop
call TestBelegt
jc @@e
BRES [PMotor.Flags],bit 2 ;ISR-Arbeit radikal stoppen
btr [PMotor.Flags],1
jnc @@e
EndInterrupts
call InformWindows
clc
@@e:
ret
endp
;*************************************
proc SMSetGear ;Übersetzungsverhältnis setzen
call TestBelegt
jc @@e
or eax,eax ;Nur setzen wenn nicht Null!
jz @@1
xchg [PMotor.Umrech1],eax
jmp @@1a
@@1: mov eax,[PMotor.Umrech1]
@@1a: or edx,edx ;Nur setzen wenn nicht Null!
jz @@2
xchg [PMotor.Umrech2],edx
jmp @@2a
@@2: mov edx,[PMotor.Umrech2]
@@2a:
@@e:
ret
endp
;*************************************
proc SMGetGear ;Übersetzungsverhältnis rücklesen
call TestBelegt
jc @@e
mov eax,[PMotor.Umrech1]
mov edx,[PMotor.Umrech2]
@@e:
ret
endp
;*************************************
proc SMSetHWE ;Hardware-Endschalter-Maske setzen
;AL=Maske, AH=XOR-Byte, Bit 16(EAX)=Schalter, ob bei Geradenbewegung
;der HWE-Schalter geprüft werden soll, DL=Offset PortA, DH=Offset PortB
;Bit17(EAX)=Schalter Dreieck statt Sinus
call TestBelegt
jc @@e
or ax,ax ;Maske setzen?
jz @@1 ;nur rücklesen wenn Null
xchg [word PMotor.HWEMask],ax
jmp @@1a
@@1: mov ax,[word PMotor.HWEMask]
@@1a:
or dx,dx ;Ports setzen?
jz @@2 ;nur rücklesen wenn Null
xchg [word PMotor.PortA],dx
jmp @@2a
@@2: mov dx,[word PMotor.PortA]
@@2a:
BRES [PMotor.Flags],bit 4 ;Prüfung ausschalten
BT eax,16
jnc @@3
BSET [PMotor.Flags],bit 4 ;Prüfung einschalten
@@3:
BRES [PMotor.Flags],bit 5 ;auf Sinus schalten
BT eax,17
jnc @@e
BSET [PMotor.Flags],bit 5 ;auf Dreieck schalten
@@e:
ret
endp
MASM
VxD_Locked_Code_Ends
;*************************************
;*** Neue API - Funktionsverteiler ***
;*************************************
VxD_Locked_Data_Seg
IDEAL
;Unterprogramm-Verteilertabelle
;Diese UP's werden mit EBX=Zeiger auf Motorstruktur angesprungen
upvt dd OFFSET32 SMNop ;"NOP"-Kommando
dd OFFSET32 SMFree ;Motorspulen stromfrei schalten
dd OFFSET32 SMSync ;Referenzfahrt
dd OFFSET32 SMMoveAbs ;Absolutbewegung
dd OFFSET32 SMMoveRel ;Relativbewegung
dd OFFSET32 SMAssign ;Belegung
dd OFFSET32 SMUnAssign ;Freigabe
dd OFFSET32 SMSetSA ;Geschw + Beschl. festlegen
dd OFFSET32 SMGetSA ;Geschw + Beschl. rücklesen
dd OFFSET32 SMSetCallback;
dd OFFSET32 SMGetCallback;
dd OFFSET32 SMGetPosition ;Momentane Position ermitteln (nach EDX)
dd OFFSET32 SMSetBounds
dd OFFSET32 SMGetBounds
dd OFFSET32 SMStop ;Not-Stopp, jedoch Spulen unter Strom
dd OFFSET32 SMSetGear ;Übersetzungsverhältnis setzen
dd OFFSET32 SMGetGear ;Übersetzungsverhältnis rücklesen
dd OFFSET32 SMSetHWE ;Hardware-Endschalter-Maske setzen
MASM
VxD_Locked_Data_Ends
VxD_Locked_Code_Seg
BeginProc MPK3D_Api
pushad
mov ax,[ebp.Client_AX] ;AX laden
cmp ah,SM_LastFunc
ja @@NoFunc
dec al
cmp al,3
jnc @@NoUnit
Debug_Out "ApiCall Fkt=#AH"
movzx ecx,ah
mov ah,size TMotor
mul ah
movzx ebx,ax
add ebx,OFFSET32 MotX
mov eax,[ebp.Client_EDX] ;1. Longint-Parameter
mov edx,[ebp.Client_ECX] ;2. LongInt-Parameter
call [upvt+ecx*4]
jc @@err ;Bei Carry AL zurückgeben
mov [ebp.Client_ECX],edx
mov [ebp.Client_EDX],eax
xor al,al ;sonst 0 = Kein Fehler
BRES [ebp.Client_Flags],CF_Mask ;Carry setzen
jmp @@NoErr
@@NoUnit:
mov al,ME_NoDriver
@@NoFunc:
mov al,ME_WrongFunction
@@Err:
BSET [ebp.Client_Flags],CF_Mask ;Carry setzen
@@NoErr:
mov [ebp.Client_AL],al
popad
ret
EndProc MPK3D_Api
IDEAL
;********************************
;*** Interruptserviceroutinen ***
;********************************
IFNDEF UseVTD
;Interruptserviceroutine für den CMOS-Tick mit nominell 1024Hz
;PE: EAX=IRQ-Griff
; EBX=VM-Griff
align 4
proc CmosIsr
sti
pushad
mov ah,0ch
call GetCMOS
BTST al,bit 6 ;Ursache=Echtzeituhr?
jz @@chain ;nein->nicht bedienen!
call intproc
popad
MASM
VxDcall VPICD_Phys_EOI
IDEAL
clc ;ISR behandelte IRQ
ret
@@chain:
Debug_Halt
popad
stc ;weiter verketten!
ret
endp CmosIsr
ENDIF
;Trapserviceroutine für Portzugriffe auf die geschützten MPK3-Adressen
;PE: EDX=Portadresse, ECX=Zugriffsart, EBX=VM-Griff, EAX=EA-Daten
align 4
proc IOHandler
MASM
Dispatch_Byte_IO Fall_Through,@@o
push edx
sub dx,[PortBase]
push eax
mov ah,dl
call InB ;Spiegelbyte oder (AH=6) Port liefern
mov dl,al
pop eax
mov al,dl
pop edx
ret
@@o:
push edx eax
sub dx,[PortBase]
mov ah,dl
call OutB ;Port schreiben, Spiegelbyte aktualisieren
pop eax edx
ret
IDEAL
endp IOHandler
;*********************
;*** Exit-Funktion ***
;*********************
proc MPK3D_Exit
MASM
Debug_Out "MPK3D_Exit"
IDEAL
mov ax,600h
call OutB ;Leistungsendstufen und Relais AUS
IFNDEF UseVTD
mov ah,0bh
call GetCMOS
BRES al,bit 6 ;CMOS Interrupt 1024Hz abschalten
call PutCMOS
mov eax,[IrqHand]
VxDcall VPICD_Physically_Mask ;IRQs aus
VxDcall VPICD_Force_Default_Behavior
ENDIF
ret
endp
MASM
VxD_Locked_Code_Ends
;******************************
;*** Installations-Funktion ***
;******************************
VxD_IData_Seg
IFNDEF UseVTD
IrqDesc VPICD_Irq_Descriptor <8,VPICD_Opt_Can_Share,OFFSET32 CmosIsr>
ENDIF
Port$: dz 'MPK3DPort'
VxD_IData_Ends
VxD_ICode_Seg
BeginProc MPK3D_Init
;* IRQ8 freischalten
;* Portadresse aus SYSTEM.INI holen
;* Ports trappen
;* Motoren zurücksetzen
Debug_Out "MPK3D_Init"
IFNDEF UseVTD
lea edi,IrqDesc
VxDcall VPICD_Virtualize_IRQ
jc @@e
mov [IrqHand],eax
VxDcall VPICD_Physically_Unmask
mov ah,0bh
call GetCMOS
BSET al,bit 6 ;CMOS Interrupt 1024Hz laufenlassen
call PutCMOS
; mov [Frequency],1024
ENDIF
movzx eax,[PortBase] ;Default
xor esi,esi ;Sektion [386Enh]
lea edi,Port$ ;Schlüssel MPK3DPort
VMMCall Get_Profile_Hex_Int ;Aus SYSTEM.INI ein Wert lesen
mov [PortBase],ax
movzx edx,ax
mov esi,OFFSET32 IOHandler
mov ecx,8 ;8 Ports überwachen
@@l: VMMcall Install_IO_Handler ;die Ports der Karte "verfeinern"
inc edx
loopd @@l
IDEAL
mov ax,600h
call OutB ;Endstufen und Relais AUS
lea ebx,[MotX]
call motor_aus
lea ebx,[MotY]
call motor_aus
lea ebx,[MotZ]
call motor_aus
clc
MASM
@@e:
ret
EndProc MPK3D_Init
VxD_ICode_Ends
END
| Detected encoding: OEM (CP437) | 1
|
|
|