;Schrittmotor-Steuerung für M20-Karte
;LogInt equ 1 ;Jeden Interrupt loggen
%MACS
%NOINCL
%TITLE "Mikroschritt-Motortreiber-VxD"
%SUBTTL "Programmkopf: Vereinbarungen usw."
include tvmm2.inc ;Generelles
include debug2.inc ;wegen Debugging
include vpicd.inc ;Interrupt-Controller
IDEAL
UMSTEP_Device_ID equ 3C0Eh ;von Microsoft bestätigt:
;>You have been assigned a device ID of 3C0Eh for your UMSTEP.386 virtual
;>device. This number is uniquely assigned to this device.
UMSTEP_Major_Ver equ 1
UMSTEP_Minor_Ver equ 10
;=============================
;Diese Programmversion arbeitet nach außen voll mit 16-bit-Registern,
;da ein gleichartiges TSR-Programm für 286er denkbar ist (die geringere
;Rechenleistung ist nicht so sehr das Problem).
ifdef DEBUG
DISPLAY !Assembliere Debugversion!
endif
Declare_Virtual_Device UMSTEP, UMSTEP_Major_Ver, UMSTEP_Minor_Ver,\
UMSTEP_Ctl, UMSTEP_Device_ID,\
Undefined_Init_Order, UMSTEP_API, UMSTEP_API
;*********************************************************
;*** Obligatorischer VxD-Botschafts-Funktionsverteiler ***
;*********************************************************
VxD_Locked_Code_Seg
BeginProc UMSTEP_Ctl
Control_Dispatch Device_Init, UMSTEP_Init
Control_Dispatch System_Exit, UMSTEP_Exit
Control_Dispatch VM_Not_Executeable, UMSTEP_VMDies
Control_Dispatch 1bh, UMSTEP_Dynamic_Init
Control_Dispatch 1ch, UMSTEP_Dynamic_Exit
clc
ret
EndProc UMSTEP_Ctl
VxD_Locked_Code_ends
;********************************************************
;*** Residenter Datenbereich und Konstantendefinition ***
;********************************************************
;Folgende Kommandos gibt es:
SM_GetVer equ 0 ;Versionsnummer holen
SM_SetMotor equ 1 ;Motor hinzufügen, Parameter setzen/ändern
;(AL=ForceAlloc-Flag, ES:BX=Mem)
SM_GetMotor equ 2 ;Motorparameter holen (AL=Handle, ES:BX=Mem)
SM_RemoveMotor equ 3 ;Motor entfernen (AL=Handle)
SM_SetIntFreq equ 4 ;Interruptfrequenz setzen (BX=Frequenz)
SM_GetIntFreq equ 5 ;Interruptfrequenz holen (BX=Frequenz)
SM_Sync equ 6 ;Referenzfahrt
SM_MoveAbs equ 7 ;Absolutbewegung nach CX:DX
SM_MoveRel equ 8 ;Relativbewegung um CX:DX
SM_Stop equ 9 ;Soforthalt
SM_Free equ 10 ;Motorspulen stromfrei schalten
SM_GetPosition equ 11 ;Position (EDX=CX:DX) und Geschwindigkeit
;(ESI=DI:SI) erfragen, BX=Frequenz)
SM_LastFunc equ 11
;{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_InvalHandle equ 50;
ME_AlreadyAssigned equ 51;
ME_OutOfHandles equ 52;
ME_OutOfMemory equ 53;
ME_InvalPointer equ 54;
ME_InvalPort equ 55;
ME_InternErr equ 56;
NomFreq equ 1024 ;Nominale Frequenz
MC_FreeOnMovEnd =1 ;Spulenstrom abschalten bei normalem Bewegungsende
MC_SyncRequired =3 ;Synchronisation vor MOVE ERFORDERLICH
MC_Signed =4 ;Vorzeichenbehafteter Mikroschritt-DAC
MC_Table =5 ;Tabelle statt Funktion (Dreieck<->Sinus)
MC_Trapeze =6 ;Trapezförmiger Strom (Trapez<->"Kurve")
MF_InMove =0 ;Motor in Bewegung
MF_InSync =1 ;Referenzfahrt erfolgt gerade
MF_Synced =2 ;Referenzfahrt war erfolgreich
MF_WarmUp =3 ;Spulenstrom wird (langsam) hochgefahren
MF_HWECheck =4 ;Endschalter angestoßen: Referenzfahrt erforderlich!?
MF_Braking =5 ;Bremse lösen und warten (InMove=1) oder
;Bremse anziehen und warten (InMove=0, WaitCount<>0)
;oder Warten, bis anderer Motor fertig (WaitCount=0)
MF_InQueue =6 ;Warteschlange mit Bewegungssegment gefüllt
MF_NegDir =7 ;Negative Bewegungsrichtung
struc TMotor
;Aufteilung nach konstantem und variablem Teil
;Der konstante Teil kann via GetMotor() abgefragt und SetMotor() gesetzt werden
Handle db ? ;Motor-Handle (0=NEUER MOTOR)
CFlags db 0 ;Konstante Motor-Flags
RefMask db 1 ;Referenztaster-Maske
RefXor db 0 ;Referenztaster-XOR-Bits
PortHWE dw ? ;Portadresse HWE
union
Ports dd ?
struc
PortA dw ? ;Portadresse Spule A (Vergleichskriterium!)
PortB dw ? ;Portadresse Spule B (Vergleichskriterium!)
ends
ends
CallbackAddr dd 0 ;16:16-Rückrufadresse
CallbackDS dw ? ;Datensegment für Callback
CallbackUser dd ? ;32-bit-Anwenderdaten
MaxSpeed dd 48*65536;Maximalgeschwindigkeit, vzl.
RefSpeed dd 12*65536;Referenz-Geschwindigkeit, vzb.!
MaxAccel dd 48 ;Maximalbeschleunigung, vzl.!
Refpoint dd 0 ;Referenzpunkt nach Sync
LeftBound dd -MaxLong;Software-Anschläge
RightBound dd MaxLong
EndMask db 1 ;Endtaster-Maske (Bereichsüberschreitung)
EndXor db 0 ;Endtaster-XOR-Bits
BrakeMask db 0 ;Bremsmagnet-Maske (Ausgabe)
BrakeXor db 0 ;Bremsmagnet-XOR-Bits (Ausgabe)
OnMask db 0 ;Hauptstromschalter-Maske (Ausgabe)
OnXor db 0 ;Hauptstromschalter-XOR-Bits (Ausgabe)
PortEAB dw ? ;Ausgabeport
;Der Callback wird gerufen mit
;EDX=CX:DX=Anwender-Daten, AL=Handle, AH=Flags, ESI=DI:SI=Position
;Der folgende variable Teil ist für die VxD-interne Benutzung vorgesehen
HWPos dw 0 ;"Hardware-Position" als Index in Sinustabelle
union
Weg df 0 ;6-Byte-Wert als Restwegspeicher
struc
WegLo dd ?
WegHi dw ?
ends
struc
WegLoW dw ?
WegMid dd ?
ends
ends
Pos dd 0 ;Aktuelle Position
Speed dd 0 ;Momentangeschwindigkeit
CallbackVM dd ? ;Virtuelle Maschine
NextMove dd ? ;Nächstes Ziel (Warteschlange)
Flags db ? ;Variable Motor-Flags
WaitCount db ? ;Warte-Zähler für Bremse oder Hochlauf
InPort db ? ;Letzter Eingabe-Wert
OutPort db ? ;Ausgabe-Byte für Ausgabeport
ends TMotor
;Diese Motor-Struktur wird in einer asynchronen Liste verwaltet.
;Das erlaubt die Ansteuerung von max. 31 Motoren gleichzeitig
PMotor equ (TMotor ebx) ;Objektzeiger ist immer EBX
TMotor_ConstLongs equ 13
TMotor_VarLongs equ 7
%SUBTTL "Handles und Tabellen"
VxD_Locked_Data_Seg
ListHand dd 0 ;Listen-Handle
IrqHand dd 0 ;IRQ-Griff
Frequency dw NomFreq ;für Zeitmessungen usw.
upcounter dd ?
;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
KurvTab:
db 128,130,131,133,134,136,138,139,141,142,144,145,147,148,150,151
db 152,154,155,157,158,159,161,162,163,164,166,167,168,170,171,172
db 173,174,175,177,178,179,180,181,182,183,184,185,186,188,189,190
db 191,192,193,193,194,195,196,197,198,199,200,201,202,202,203,204
db 205,206,207,207,208,209,210,210,211,212,213,213,214,215,216,216
db 217,218,218,219,219,220,221,221,222,223,223,224,224,225,225,226
db 226,227,227,228,229,229,230,230,230,231,231,232,232,233,233,234
db 234,234,235,235,236,236,236,237,237,238,238,238,239,239,239,240
db 240,240,241,241,241,242,242,242,242,243,243,243,244,244,244,244
db 245,245,245,245,245,246,246,246,246,247,247,247,247,247,248,248
db 248,248,248,248,249,249,249,249,249,249,249,250,250,250,250,250
db 250,250,251,251,251,251,251,251,251,251,251,252,252,252,252,252
db 252,252,252,252,252,252,252,253,253,253,253,253,253,253,253,253
db 253,253,253,253,253,253,253,254,254,254,254,254,254,254,254,254
db 254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,255
db 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255
VxD_Locked_Data_Ends
;*****************************
;*** Sinustabellen-Zugriff ***
;*****************************
VxD_Locked_Code_Seg
;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: Funktionsergebnis, Zentrierung wie gewünscht
;VR: AX
inc ah
GetSin:
test ah,bit 0
jz @@1 ;1. oder 3. Quadrant
not al ;Bit-Komplement bilden
@@1: BTST [PMotor.CFlags],bit MC_Table ;Sinus/Dreieck-Schalter
BT [dword PMotor.CFlags],MC_Trapeze
jnz @@dreieck
push ebx
lea ebx,[SinTab]
jnc @@1a
lea ebx,[KurvTab]
@@1a: xlat ;Tabellenzugriff
pop ebx
jmp FlipAL
@@dreieck:
jc @@trapez
stc ;0-->80, 1-->80, 2-->81, 3-->81, 4-->82,...
rcr al,1 ;FD-->FE, FE-->FF, FF-->FF (Erster Quadrant)
jmp FlipAL
@@trapez:
add al,80h ;00-->80, 01-->81, ... 7F-->FF,
jnc FlipAL
sbb al,al ;AL:=FF, 80-->FF, 81-->FF, .. FF-->FF
FlipAL: test ah,bit 1 ;3./4. Quadrant
jz FlipSign ;nein, positiv lassen
not al ;ja, 80-->7F, 81-->7E,... FF-->00
FlipSign:
;Die zweite Motorsteuerung arbeitet mit Vorzeichen/Betrag (vereinfachte
;Hardware?) - deshalb "Schnellemachefix-Patch"
BTST [PMotor.CFlags],bit MC_Signed
jz @@e
BTST al,bit 7
jnz @@e
xor al,7Fh
@@e: ret
endp GetCos
;*****************************
;*** Portzugriffe auf CMOS ***
;*****************************
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
;*******************************
;*** Direkte Motor-Steuerung ***
;*******************************
;Ausschalten des Stroms durch die Spulen
;PE: EBX: Objektzeiger
;VR: AX
proc Motor_Aus
mov al,128
call FlipSign
mov edx,[PMotor.Ports]
out dx,al
shr edx,16
out dx,al
BRES [PMotor.Flags],bit MF_Synced
ret
endp Motor_Aus
;Allokieren des Motors; Spulenstrom sicherheitshalber ausschalten
;PE: EBX: Objektzeiger
;VR: AX
proc Motor_Init
pushad
call Motor_Aus
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
popad
ret
endp Motor_Init
;Motor um vzb. Betrag bewegen, dieser sollte -256..256 nicht überschreiten.
;Dabei wird die Positions-Arbeitszelle mitgeführt
;PE: EAX: vzb. Betrag (möglichst -256..256)
; EBX: Objektzeiger
;VR: EDX,EAX
proc motor_move
BTST [PMotor.Flags],bit MF_Synced
jz @@1 ;Position NICHT mitführen, wenn unbekannt
@@0: add [PMotor.Pos],eax
@@1: add [PMotor.HWPos],ax
mov ax,[PMotor.HWPos]
mov edx,[PMotor.Ports]
push eax
call GetCos
out dx,al
pop eax
call GetSin
shr edx,16
out dx,al
ret
endp motor_move
;****************************
;*** Periodische Aktionen ***
;****************************
proc intWarmup
;Voraussetzung: HWPos=0, Spulen stromlos
;WaitCount zählt von 255 (Strom Null) bis 0 (Strom maximal)
;Zweck: Vermeiden des "Einrüttelns" des Motors beim Strom zuschalten
IFDEF LogInt
Debug_Out "IntWarmup "
ENDIF
movzx eax,[PMotor.WaitCount]
mov edx,[PMotor.Ports]
call GetCos
out dx,al
dec [PMotor.WaitCount]
ret
endp intWarmup
proc intSync
IFDEF LogInt
Debug_Out "IntSync "
ENDIF
mov eax,[dword PMotor.RefMask]
call IsSenseOn ;Hardware-Endschalter abfragen
jnz @@1 ;Treffer - Schalter betätigt
xor eax,eax
BTR [PMotor.Flags],MF_NegDir ;(+) Richtung ZUM Taster
jc @@3 ;im Wendepunkt 1x aussetzen
@@5: mov eax,[PMotor.RefSpeed]
@@3: call MoveBySpeed
ret
@@1:
BTST [PMotor.Flags],bit MF_NegDir ;(-) Richtung VOM Taster?
jnz @@5 ;War's schon: weiter rückwärts!
;neu: kein "Einrütteln" in exakte Position, sondern
;Aufaddieren der Hardware-Position zum Referenzpunkt
mov eax,[dword PMotor.HWPos] ;10-Bit-Zahl holen
shl eax,22
sar eax,22 ;Einfachste Möglichkeit der Vorzeichenerweiterung
add eax,[PMotor.Refpoint]
mov [PMotor.Pos],eax ;Das ist nun die Position
@@br equ %((bit MF_InMove) or (bit MF_InSync) or (bit MF_HWECheck))
BRES [PMotor.Flags],@@br
BSET [PMotor.Flags],bit MF_Synced
call InformWindows
ret
endp intSync
proc intMove
IFDEF LogInt
Debug_Out "IntMove "
ENDIF
mov eax,[PMotor.WegLo]
or eax,[PMotor.WegMid] ;Nichts mehr tun?
jz @@1
mov eax,[dword PMotor.EndMask]
call IsSenseOn ;Hardware-Endschalter abfragen
jnz @@3
@@2: call CalcNewSpeed
call MoveBySpeed
ret
@@3: xor eax,eax
mov [PMotor.WegLo],eax
mov [PMotor.WegHi],ax
mov [PMotor.Pos],eax
BRES [PMotor.Flags],bit MF_Synced
BSET [PMotor.Flags],bit MF_HWECheck
@@1:
mov [PMotor.Speed],eax
BRES [PMotor.Flags],bit MF_InMove ;Bewegungssegment ENDE
call InformWindows
ret
endp intMove
proc intproc ;Das Innere des Timer-Interrupts
@@a equ %((bit MF_WarmUp)or(bit MF_Braking)or(bit MF_InMove)or(bit MF_InSync))
mov esi,[ListHand]
cli
VMMCall List_Get_First
jmp @@e
@@l: test [(TMotor eax).Flags],@@a ;Aktion steht an?
jz @@na ;Zum nächsten Element!
sti
push esi
xchg ebx,eax ;EBX=Objektzeiger
mov al,[PMotor.Flags]
test al,bit MF_WarmUp
jnz @@nw
call intWarmup
jmp @@o
@@nw: test al,bit MF_Braking
jnz @@nb
call intBraking
jmp @@o
@@nb: test al,bit MF_InSync
jnz @@ns
call intSync
jmp @@o
@@ns: call intMove ;bleibt übrig
@@o: xchg eax,ebx
pop esi
cli
@@na: VMMCall List_Get_Next
jnz @@l
@@e: sti
inc [upcounter]
ret
endp intproc
;*******************************
;*** Interruptserviceroutine ***
;*******************************
;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
VxDcall VPICD_Phys_EOI
clc ;ISR behandelte IRQ
ret
@@chain:
Debug_Halt
popad
stc ;weiter verketten!
ret
endp CmosIsr
;*****************************************
;*** Aktions-Anstöße für Sync und Move ***
;*****************************************
proc StartSegment
;PE: EAX=Zielposition
cli
BTST [PMotor.Flags].bit MF_InMove
jz @@n
mov [PMotor.NextMov],eax ;Einfach überschreiben
BSET [PMotor.Flags],bit MF_InQueue
sti
ret
@@rb equ %((bit MF_NegDir) or (bit MF_InSync))
@@n: BRES [PMotor.Flags],@@rb ;Positiv annehmen, keine RefFahrt
sub eax,[PMotor.Pos] ;Differenz
jge @@1
neg eax ;Betrag bilden
BSET [PMotor.Flags],bit MF_NegDir ;und Richtung merken
@@1: push eax
xor eax,eax
mov [PMotor.Speed],eax ;Geschwindigkeit nullsetzen
mov [PMotor.WegLo],eax
pop eax
mov [PMotor.WegMid],eax ;und Weg entsprechend setzen
BSET [PMotor.Flags],bit MF_InMove ;Und LOS!
sti
ret
endp
proc StartReffahrt
mov [PMotor.Speed],0
@@sb equ %((bit MF_InMove) or (bit MF_InSync) or (bit MF_NegDir))
BSET [PMotor.Flags],@@sb ;ISR werkeln lassen
ret
endp
;***************************************
;*** Rückruf-Funktionen für Callback ***
;***************************************
proc PostEventProc
mov ebx,edx ;geordnete Verhältnisse
cmp [PMotor.CallbackAddr],0
jz @@e
Debug_Halt
Push_Client_State USES_EDI
VMMCall Begin_Nest_Exec
mov eax,[PMotor.CallbackUser]
mov [PClient._EDX],eax
shr eax,16
mov [PClient._ECX],eax
mov eax,[PMotor.Pos]
mov [PClient._ESI],eax
shr eax,16
mov [PClient._EDI],eax
mov al,[PMotor.Handle]
mov ah,[byte PMotor.Flags]
mov [PClient._EAX],eax
mov ax,[PMotor.CallbackDS]
mov [PClient._DS],ax
movzx edx,[word LOW PMotor.CallbackAddr]
mov cx,[word HIGH PMotor.CallbackAddr]
VMMCall Simulate_Far_Call
VMMCall Resume_Exec
VMMCall End_Nest_Exec
Pop_Client_State USES_ESI
@@e: ret
endp
proc InformWindows c
uses ebx,ecx,edx
mov edx,ebx
mov eax,High_Pri_Device_Boost
mov ebx,[PMotor.CallbackVM]
LD ecx,<PEF_Wait_For_STI or PEF_Wait_Not_Crit or PEF_Always_Sched>
lea esi,[PostEventProc]
VMMCall Call_Priority_VM_Event
mov al,ME_InternErr ;falls CY=1
ret
endp
;***********************************************
;*** Berechnung des Geschwindigkeitsverlaufs ***
;***********************************************
;PE: EAX=vzl. (oder auch vzb.) Geschwindigkeit in Nanoschritten
; EBX=Objektzeiger
;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],MF_NegDir ;Richtungs-Bit nach CY
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, E(EDX) immer 0)
;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 dx,[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 dx,[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
%SUBTTL "V86- und PM-API"
;*********************************************
;*** Testfunktionen mit Fehlercoderückgabe ***
;*********************************************
;Testet, ob HWE betätigt wurde, und ggf. Fehler melden
proc TestHWE
BTST [PMotor.Flags],bit MF_HWECheck ;Endschalter betätigt?
jz @@e ;nein, okay
mov al,ME_HardEnd
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 MF_InMove ;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, und lädt anschließend EAX aus Client_CX:DX
proc TestReferenz
BTST [PMotor.CFlags],bit MC_SyncRequired
jz @@w
BTST [PMotor.Flags],bit MF_Synced ;Ungültig?
jnz @@w ;Gültig!
mov al,ME_NoRef
stc
@@e: ret
@@w: mov ax,[PClient._CX]
shl eax,16
mov ax,[PClient._DX]
clc
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
;**********************
;*** API-Funktionen ***
;**********************
proc SMGetVer
mov [PClient._DX],UMSTEP_Version
clc
ret
endp
;*************************************
proc SMSetMotor
;PE: Client_ES:BX=Konstante Motor-Struktur mit
; Handle=0 für neuen Motor (ggf. Zwangszuweisung bei gleichen Portadressen)
; Handle<>0: Daten für vorhandenen Motor neu setzen
; Komponente "Handle" wird ggf. vom VxD verändert
; Client_AL=0: Bei auffindbarer gleicher Portadresse Fehler!
Client_Ptr_Flat ebx,ES,BX,USES_EAX
add eax,1
mov al,ME_InvalPointer
jc @@e ;Ungültige Adresse in ES:BX
mov al,ME_InvalPort
cmp [PMotor.PortA],1
jc @@e ;Ungültige PortA-Adresse
cmp [PMotor.PortB],1
jc @@e ;Ungültige PortB-Adresse
;Erst einmal passenden Listeneintrag suchen
cli
mov esi,[ListHand]
VMMCall List_Get_First
jz @@o1 ;Noch leere Liste
@@l1: push eax
mov edi,eax ;Listen-Knoten
mov al,[PMotor.Handle]
or al,al ;Neuer Motor?
jz @@n1
cmp [(TMotor edi).Handle],al ;Gleiches Handle?
jne @@nf
jmp @@e1
@@n1: ;Portadressen vergleichen - bei Gleichheit Fehler oder Zwangszuweisung
mov eax,[PMotor.Ports]
cmp [(TMotor edi).Ports],eax
jne @@nf
cmp [PClient._AL],1 ;Fehlermeldung bei gleichen Ports erzwingen?
mov al,ME_AlreadyAssigned
jc @@e2 ;ja, raus!
mov al,[(TMotor edi).Handle]
mov [PMotor.Handle],al ;Handle rückmelden bei gleichen Ports
@@e1: pop eax
call EditNode
clc
jmp @@e
@@e2:
pop eax
jmp @@e ;raus mit Fehler
@@nf:
pop eax
VMMCall List_Get_Next
jnz @@l1
@@o1:
cmp [PMotor.Handle],0 ;Null?
stc
mov al,ME_InvalHandle
jnz @@e ;Nicht null: Fehler!
bsf edx,[HandlesUsed]
mov al,ME_OutOfHandles
jz @@e ;Fehler - nichts frei!
btr [HandlesUsed],edx ;Bit als belegt kennzeichnen
mov [PMotor.Handle],dl ;Neues Handle rückmelden
@@2:
VMMCall List_Allocate ;Listen-Knoten beschaffen
push eax
xchg edi,eax
mov al,ME_OutOfMemory
jc @@e2
LD ecx,TMotor_ConstLongs+TMotor_VarLongs
xor eax,eax
rep stosd ;Gründlich löschen
pop eax
call EditNode
mov ebx,eax
call Motor_Init
VMMCall List_Attach ;und dann einfügen
clc
@@e: sti
ret
endp
proc GetPMotor
;PE: AL=Handle
;PA: EBX=Zeiger auf Motor-Struktur
; EDX=Handle (nullerweitert)
; ESI=Listen-Handle
; EAX=EBX=Motor-Struktur
; CY=1: Nicht gefunden, dann AL=Fehlercode ME_
;VR: ESI,EAX,EBX,EDX
movzx edx,al
add al,-32
mov al,ME_InvalHandle
jc @@e ;Fehler: Handle>31
bt [HandlesUsed],edx
jc @@e ;Fehler: Unbenutztes Handle!
cli
mov esi,[ListHand]
VMMCall List_Get_First
jz @@o1 ;Noch leere Liste
@@l1: mov ebx,eax
cmp [PMotor.Handle],dl
jz @@e ;gefunden! CY=0
VMMCall List_Get_Next
jnz @@l1
@@o1: mov al,ME_InternErr
stc
sti
@@e: ret
endp GetPMotor
proc MakeStatusOn
;Setzt einen Status (gebremst, eingeschaltet)
;PE: AL=Maske, AH=XOR, EBX=PMotor-Zeiger, CLI; PA: -; VR: AX,DX
or al,al
jz @@e ;nichts zu tun!
xor ah,al
not al
and al,[PMotor.OutPort]
or al,ah
mov dx,[PMotor.PortEAB]
out dx,al
mov [PMotor.OutPort],al
@@e: ret
endp MakeStatusOn
proc MakeStatusOff
;Löscht einen Status (gebremst, eingeschaltet)
;PE: AL=Maske, AH=XOR, EBX=PMotor-Zeiger, CLI; PA: -; VR: AL,DX
or al,al
jz @@e ;nichts zu tun!
not al
and al,[PMotor.OutPort]
xor al,ah
mov edx,[dword PMotor.PortEAB]
out dx,al
mov [PMotor.OutPort],al
@@e: ret
endp MakeStatusOff
proc IsStatusOn
;Prüft einen Status (gebremst, eingeschaltet)
;PE: AL=Maske, AH=XOR, EBX=PMotor-Zeiger; PA: NZ: Ein, Z: Aus; VR: AH
xor ah,[PMotor.OutPort]
test al,ah
ret
endp IsStatusOn
proc IsSenseOn
;Prüft einen Taster (End- oder Referenztaster)
;PE: AL=Maske, AH=XOR, EBX=PMotor-Zeiger
;PA: AL=In-Byte, NZ: On, Z: Off; VR: EAX,ECX,EDX
or al,al
jz @@e ;Langsamen IN-Befehl umgehen, wenn unnötig
xchg ecx,eax
mov edx,[dword PMotor.PortHWE]
in al,dx
xor ch,al
test cl,ch
@@e: ret
endp IsSenseOn
proc EditNode
;PE: EAX=Knoten (Ziel), EBX=User-Struktur(Quelle)
; Die aktuelle VM wird zur Callback-VM erklärt
pushad
xchg edi,eax
xchg esi,ebx
LD ecx,TMotor_ConstLongs
rep movsd
VMMCall Get_Cur_VM_Handle ;VM-Handle setzen
mov [((TMotor edi)-4*TMotor_ConstLongs).CallbackVM],ebx
popad
ret
endp EditNode
;*************************************
proc SMGetMotor
;PE: Client_AL=AL=Handle
; Client_ES:BX=Stückel Speicher
call GetPMotor
jc @@e ;Fehler: Nicht zu finden!
Client_Ptr_Flat eax,ES,BX
mov edi,eax
add eax,1
mov al,ME_InvalPointer
jc @@e ;Ungültige Adresse in ES:BX
mov esi,ebx
LD ecx,TMotor_ConstLongs
rep movsd ;Daten liefern
clc
@@e: ret
endp
;*************************************
proc SMRemoveMotor
;PE: Client_AL=AL=Handle
call GetPMotor
jc @@e
call Motor_Aus ;Motor freischalten
bts [HandlesUsed],edx
mov eax,ebx
VMMCall List_Remove
mov al,ME_InternErr
jc @@e
xchg eax,ebx
VMMCall List_Deallocate
clc
@@e: ret
endp
;*************************************
proc SMSetIntFreq ;Dummy: Nichts tun!
mov ax,[PClient._BX]
clc
@@e: ret
endp
;*************************************
proc SMGetIntFreq
mov ax,[Frequency]
mov [PClient._BX],ax
clc
@@e: ret
endp
;*************************************
proc SMSync ;Referenzfahrt
call GetPMotor
jc @@e
call TestStill
jc @@e
call StartReffahrt
@@e: ret
endp
;*************************************
proc SMMoveAbs ;Absolutbewegung
call GetPMotor
jc @@e
call TestReferenz
jc @@e
call TestInBounds
jc @@e
call StartSegment
@@e: ret
endp
;*************************************
proc SMMoveRel ;Relativbewegung
call GetPMotor
jc @@e
call TestReferenz
jc @@e
add eax,[PMotor.Pos]
call TestInBounds
jc @@e
call StartSegment
@@e: ret
endp
;*************************************
proc SMStop
call GetPMotor
jc @@e
btr [dword PMotor.Flags],MF_InMove;ISR-Arbeit radikal stoppen
jnc @@e
call InformWindows ;Callback rufen, wenn in Bewegung!
@@e: ret
endp
;*************************************
proc SMFree ;Motorspulen stromfrei schalten, Bremse lösen
call GetPMotor
jc @@e
btr [dword PMotor.Flags],MF_InMove;ISR-Arbeit radikal stoppen
jc @@stop ;Auf jeden Fall Sync verlieren
BTST [PMotor.CFlags],bit MC_FreeOnMovEnd
jnz @@s1 ;in diesem Fall Sync belassen
jmp @@s0 ;ansonsten auch SYNC=0
@@stop: call InformWindows
@@s0: BRES [PMotor.Flags],bit MF_InSync
@@s1: call Motor_Aus
mov eax,[dword PMotor.BrakeMask]
call ReleaseBit
@@nobrake:
clc
@@e: ret
endp
;*************************************
proc SMGetPosition ;liefert Position und Geschwindigkeit
call GetPMotor
jc @@e
cli ;Ein zusammenhängendes Paar erwischen!
mov eax,[PMotor.Pos]
mov edx,[PMotor.Speed]
mov cl,[byte PMotor.Flags]
sti
mov [PClient._EDX],eax
shr eax,16
mov [PClient._ECX],eax
xchg eax,edx
mov [PClient._ESI],eax
shr eax,16
mov [PClient._EDI],eax
mov [PClient._AH],cl
jmp SMGetIntFreq
@@e:
ret
endp
;*************************************
VxD_Locked_Code_Ends
;*************************************
;*** Neue API - Funktionsverteiler ***
;*************************************
VxD_Locked_Data_Seg
HandlesUsed dd 0FFFFFFFEh
;Bitmaske für verwendete Handles
;0=belegt, 1=frei; Bit0 ist immer "belegt"
;Suche freier Bits erfolgt mit BSF
;Unterprogramm-Verteilertabelle
upvt dd OFFSET SMGetVer ;Versionsnummer holen
dd OFFSET SMSetMotor ;Motor hinzufügen, Parameter setzen
dd OFFSET SMGetMotor ;Motorparameter holen
dd OFFSET SMRemoveMotor ;Motor entfernen
dd OFFSET SMSetIntFreq ;Interruptfrequenz setzen
dd OFFSET SMGetIntFreq ;Interruptfrequenz holen
dd OFFSET SMSync ;Referenzfahrt
dd OFFSET SMMoveAbs ;Absolutbewegung
dd OFFSET SMMoveRel ;Relativbewegung
dd OFFSET SMStop ;Soforthalt
dd OFFSET SMFree ;Motorspulen stromfrei schalten
dd OFFSET SMGetPosition ;Position und Geschw. erfragen
VxD_Locked_Data_Ends
VxD_Locked_Code_Seg
BeginProc UMSTEP_Api
pushad
mov ax,[PClient._AX] ;AX laden
cmp ah,SM_LastFunc
ja @@NoFunc
Debug_Out "ApiCall Fkt=#AH"
movzx ecx,ah
call [upvt+ecx*4]
jc @@err ;Bei Carry AL zurückgeben
xor al,al ;sonst 0 = Kein Fehler
BRES [PClient._Flags],CF_Mask ;Carry löschen
jmp @@NoErr
@@NoFunc:
mov al,ME_WrongFunction
@@Err:
BSET [PClient._Flags],CF_Mask ;Carry setzen
@@NoErr:
mov [PClient._AL],al
popad
ret
EndProc UMSTEP_Api
;*********************
;*** Exit-Funktion ***
;*********************
proc UMSTEP_VMDies
;Wenn eine VM stirbt, müssen zugehörige Motoren "entfernt" werden!
mov esi,[ListHand]
mov edi,ebx ;Aktuelle VM
pushf
cli
@@vorn: VMMCall List_Get_First
jz @@o
@@l: xchg ebx,eax
cmp [PMotor.CallbackVM],edi
jnz @@nm
call Motor_Aus ;Freischalten!
mov eax,ebx
VMMCall List_Remove
xchg eax,ebx
VMMCall List_Deallocate
jmp @@vorn ;Liste noch einmal von vorn durchgehen!
@@nm: xchg eax,ebx
VMMCall List_Get_Next
jnz @@l
@@o: popf
ret
endp UMSTEP_VMDies
proc UMSTEP_Exit
UMSTEP_Dynamic_Exit:
Debug_Out "UMSTEP_Exit"
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
mov esi,[ListHand]
VMMCall List_Destroy
ret
endp
VxD_Locked_Code_Ends
;******************************
;*** Installations-Funktion ***
;******************************
VxD_IData_Seg
IrqDesc VPICD_Irq_Descriptor <8,VPICD_Opt_Can_Share,OFFSET32 CmosIsr>
VxD_IData_Ends
VxD_ICode_Seg
BeginProc UMSTEP_Init
;* IRQ8 freischalten
;* Portadresse aus SYSTEM.INI holen
;* Ports trappen
;* Motoren zurücksetzen
Debug_Out "UMSTEP_Init"
UMSTEP_Dynamic_Init:
LD eax,LF_Async
LD ecx,<size TMotor>
VMMCall List_Create
jc @@e
mov [ListHand],esi
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 ;fest in diesem Programm
clc
@@e:
ret
EndProc UMSTEP_Init
VxD_ICode_Ends
END
| Vorgefundene Kodierung: OEM (CP437) | 1
|
|
|