1. No category

Institut “Joˇzef Stefan” Ljubljana, Slovenija
yy
yy
y y
y
DP 10232
Phantom Omni driver for Matlab/Simulink
Phntom Omni gonilnik za Matlab/Simulink
Tadej Petriˇc
Ljubljana, junij 2009
Kazalo
1
2
Uvod
3
1.1
Specifikacije haptiˇcnega vmesnika Phantom Ohmni . . . . . . . . . . . . . . .
3
1.2
Zahteve . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
1.3
Namestitev . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
1.4
Prevajanje izvorne kode . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
Matlab/Simulink knjiˇznica
6
2.1
Omni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7
2.2
Omni MaxForce . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
2.3
Omni Torque . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
2.4
Vector to matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
3
Primer
14
4
Izvorna koda za gonilnik: Omni
15
2
1 Uvod
Haptiˇcni vmesniki so robotske naprave, ki omogoˇcajo interakcijo z okoljem. Okolje je lahko bodisi navidezno ali telemanipulacijsko. Haptiˇcni vmesniki so zato uporabni predvsem za naloge,
ki se obiˇcajno izvajajo roˇcno. Primer takˇsne naloge je raziskovanje ali manipuliranje objektov.
Naprava v sploˇsnem dobiva motoriˇcne ukaze od uporabnika, vraˇca pa silo oziroma navor kot
posledico interakcije z okoljem. Tem interakcijam so pogosto dodane sˇe dodatne stimulacije.
To so naprimer cˇ utila, vid in sluh. Primer haptiˇcne naprave je na sliki 1.
Slika 1: Haptiˇcna naprava podjetja SensAble, znamka Phantom Omni
1.1 Specifikacije haptiˇcnega vmesnika Phantom Ohmni
Force feedback workspace
Footprint
Weight
Range of motion
Nominal position resolution
Backdrive friction
Maximum exertable force at nominal position
Continuous exertable force (24 hrs.)
Stiffness
Inertia (apparent mass at tip)
Force feedback
Position sensing [Stylus gimbal]
Interface
Supported platforms
OpenHaptics Toolkit compatibility
3
160 W x 120 H x 70 D mm.
168 W x 203 D mm.
1.36kg
Hand movement pivoting at wrist
0.055 mm.
0.26 N
3.3 N
0.88 N
X axis > 1.26 N/mm
Y axis > 2.31 N/mm
Z axis > 1.02 N/mm
45 g
x, y, z
x, y, z (digital encoders)
Pitch, roll, yaw (linearity potentiometers)
IEEE-1394 FireWire
Intel or AMD-based PCs
Yes
1.2 Zahteve
Omni gonilnik zahteva programsko okolje Matla/Simulink ter OpenHaptic Toolkit. Preizkuˇseno
je bilo na sistemih:
• Operacijski sistem: Microsoft Windows Vista (Service Pack 1)
• MATLAB Version 7.7.0.471 (R2008b)
• OpenHaptics Toolkit 3 (Academic edition)
Ter na sistemu:
• Operacijski sistem: Microsoft Windows XP (Service Pack 2)
• MATLAB Version 7.5 (R2007b)
• OpenHaptics Toolkit 3 (Academic edition)
1.3 Namestitev
Za zadnjo verzijo gonilnikov za povezavo haptiˇcne naprave Phantom Omni in Matlab/Simulinka je potrebno poslati e-mail na naslov: [email protected], kjer boste prejeli Omni_matlabXX.zip.
Arhiv Omni_matlabXX.zip vsebuje naslednje datoteke:
•
•
•
•
•
•
•
•
..\Omni_matlab\IJS_omni.mdl
..\Omni_matlab\omni_driver.mexw32
..\Omni_matlab\omni_driver_maxforce.mexw32
..\Omni_matlab\omni_driver_torque.mexw32
..\Omni_matlab\source\omni_driver.c
..\Omni_matlab\source\omni_driver_maxforce.c
..\Omni_matlab\source\omni_driver_torque.c
..\Omni_matlab\demo\omni_demo.mdl
Datoteke je potrebno dodati v MATLAB PATH. Priloˇzena izvorna koda je pod licenco GPL,
verzije 3.
1.4 Prevajanje izvorne kode
Izvorna koda posameznega modula je v mapi ..\Omni_matlab\source\. Prevaja se z
prevajalnikom c kode (preizkuˇseno s prevajalnikom Microsoft Visual C++ 2005). Omogoˇcit je
potrebno dostop prevajalnika, do zaglavnih dokumentov v mapah:
• ..\SensAble\3DTouch\include\
• ..\SensAble\3DTouch\utilities\include\
4
in uporabiti knjiˇzice (.lib), ki se nahajajo v:
• ..\SensAble\3DTouch\lib\win32\
• ..\SensAble\3DTouch\utilities\lib\Win32\ReleaseAcademicEdition\
Kar storimo tako, da uredimo datoterko:
• ..\Users\user\AppData\Roaming\MathWorks\MATLAB\R2008b\mexopts.bat.
Vanjo dodamo poti do zaglavnih datotek v vrstico set INCLUDE in poti do knjiˇznic v vrstico
set LIB. Primer popravljenega dela datoteke mexopts.bat:
1
3
5
7
9
11
13
...
rem ********************************************************************
rem General parameters
rem ********************************************************************
set
set
set
set
set
MATLAB=%MATLAB%
VS80COMNTOOLS=%VS80COMNTOOLS%
VSINSTALLDIR=C : \ Program Files\Microsoft Visual Studio 8
VCINSTALLDIR=%VSINSTALLDIR%\VC
PATH=%VCINSTALLDIR%\BIN\;%VCINSTALLDIR%\PlatformSDK\bin;%VSINSTALLDIR%\Common7\IDE;%←VSINSTALLDIR%\SDK\v2 . 0 \ bin;%VSINSTALLDIR%\Common7\Tools;%VSINSTALLDIR%\Common7\Tools\bin←;%VCINSTALLDIR%\VCPackages;%MATLAB_BIN%;%PATH%
set INCLUDE=%VCINSTALLDIR%\ATLMFC\INCLUDE;%VCINSTALLDIR%\INCLUDE;%VCINSTALLDIR%\PlatformSDK\←INCLUDE;%VSINSTALLDIR%\SDK\v2 . 0 \ include ; c : \ program Files\sensAble\3DTouch\include \ ; c : \ ←program Files\sensAble\3DTouch\utilities\include\;%INCLUDE%
set LIB=%VCINSTALLDIR%\ATLMFC\LIB;%VCINSTALLDIR%\LIB;%VCINSTALLDIR%\PlatformSDK\lib;%←VSINSTALLDIR%\SDK\v2 . 0 \ lib;%MATLAB%\ e x t e r n \lib\win32 ; c : \ program Files\sensAble\3DTouch\←utilities\lib\Win32\ReleaseAcademicEdition \ ; c : \ program Files\sensAble\3DTouch\lib\win32←;%LIB%
set MW_TARGET_ARCH=win32
...
V enajsto vrstico smo dodali poti do zaglavnih datotek. V dvanajsto vrstico smo dodali poti do
knjiˇznic potrebnih za prevajanje gonilnika.
Sintaksa za prevajanje gonilnika v Matlabu je:
• >> mex omni_driver*.c hd.lib hdu.lib
5
2 Matlab/Simulink knjiˇznica
Matlab/Simulink knjiˇznico sestavljajo sˇtirje elementi. Pri cˇ emer so trije elementi samostojni
ˇ
gonilniki za haptiˇcno napravo Phantom Omni z razliˇcnimi lastnostmi. Cetrti
element knjiˇznice
transformira vektor v matriko. Vsi trije gonilniki so v celoti samostojni. Razlikujejo se glede
na obliko vhodnega signala. Prvi gonilnik imenovan Omni T orque ima vhodni signal velikosti
3x1. Vrednost vhodnega signala predstavlja navore na posameznih oseh haptiˇcne naprave. Gonilnika Omni in Omni M axF orce imata vhodni signal velikosti 3x1. Vrednost tega vektorja
pa predstavlja sile v karteziˇcnih koordinatah. Gonilnika se razlikujeta po omejitvi maksimalne
sile. Ta je pri gonilniku Omni nastavljena na takˇsno vrednost, ki omogoˇca 24 urno delovanje naprave Phantom Omni pod konstantno obremenitvijo. Pri gonilniku Omni M axF orce pa
je dovoljena maksimalna sila, ki jo naprava zmore. Slika 2 prikazuje knjiˇznico z gonilniki za
haptiˇcno napravo Phantom Ohmi.
(c) 2009Tadej Petric
The Jozef Stefan Institute
Slovenia
Drivers for Phantom Omni
Raw
SensAble
Raw
p
Phantom
Omni
T
M
Torque
p
p
Phantom
Omni
pd
pd
T(1x16) T(4x4)
pd
M
q
F
MaxForce
> WARNING <
But
Omni Torque
Raw
SensAble
SensAble
T
F
F
T
Phantom
Omni
Vector > Matrix
F
q
q
But
But
Omni MaxForce
Omni
Slika 2: Knjiˇznica gonilnikov za haptiˇcno napravo Phantom Omni v okolju Matlab/Simulink
S pomoˇcjo teh gonilnikov se odpirajo nove moˇznosti interakcije s programskim okoljem Matlab/Simulink. Interakcija z okoljem je lahko virtualna ali pa se interakcija z realnim okoljem
izvaja na daljavo s pomoˇcjo robota(teleoperiranje). Grafiˇcen primer teleoperacije ter povezave
med posameznimi elementi prikazuje slika 3. Sile in navori, ki se pojavijo na vrhu robota,
se prenaˇsajo preko programskega okolja Matlab/Simulink na cˇ loveˇskega operaterja s pomoˇcjo
haptiˇcne napreve.
MATLAB
Slika 3: Povezava haptiˇcne naprave, Matlab/Simulink okolja in robotske roke
6
2.1 Omni
Namen
Gonilnik haptiˇcne naprave Phantom Omni. Vhodni signal je
sila.
Knjiˇznica
Omni_matlab
Opis
Gonilnik za vzpostavitev povezave med hapriˇcno napravo
Phantom Omni in programskim paketom Matlab/Simulink.
Vhod je vektor sile F dimenzije 3x1. Vrednosti vektorja so
velikost sile v smereh karteziˇcnih koordinat. Maksimalna velikost sile je omejena na vrednost, ki jo naprava lahko oddaja 24
ur. Vrednosti sile so v enoti N .
Gonilnik sestavlja 7 izhodnih signalov v naslednjem vrstnem
redu:
Raw
p
SensAble
F
Phantom
Omni
pd
T
F
q
But
Omni
Parametri
okno
in
1. Raw velikosti 6x1. Predstavlja neobdelane vrednosti inkrementalnih senzorjev v posameznih osi.
2. p velikosti 3x1. Predstavlja poloˇzaj vrha haptiˇcne naprave v karteziˇcnih koordinatah. Vrednosti so v mm.
3. pd velikosti 3x1. Predstavlja hitrost gibanja vrha robota
v karteziˇcnih koordinatah v enoti mm/s.
4. T velikosti 16x1. Predstavlja vrednosti transformacijske
matrike 0 T6 .
5. F je dimenzij 3x1. Predstavlja vrednosti trenutne sile, ki
jo naprava oddaja v enoti N .
6. q je vektor dimenzije 6x1. Opisuje vrednosti notranjih
koordinat v enoti rad.
7. But velikosti 2x1. Predstavlja stanje stikal na roˇcaju
haptiˇcne naprave.
ˇ
korak (tipiˇcno 0.01s)
pogovorno Casovni
7
Karakteristike
•
•
•
•
Direct Feedthrough: Da
Stanja: Ne
Velikost vektorjev: Konstantna
Zero Crossing: Ne
8
2.2 Omni MaxForce
Namen
Gonilnik haptiˇcne naprave Phantom Omni. Vhodni signal je
sila.
Knjiˇznica
Omni_matlab
Opis
Gonilnik za vzpostavitev povezave med hapriˇcno napravo
Phantom Omni in programskim paketom Matlab/Simulink.
Vhod je vektor sile F dimenzije 3x1. Vrednosti vektorja so
velikost sile v smereh karteziˇcnih koordinat. Maksimalna velikost ni omejena. Vrednosti sile so v enoti N .
Gonilnik sestavlja 7 izhodnih signalov v naslednjem vrstnem
redu:
Raw
SensAble
F
p
Phantom
Omni
pd
MaxForce
F
> WARNING <
T
q
But
Omni MaxForce
Parametri
okno
in
1. Raw velikosti 6x1. Predstavlja neobdelane vrednosti inkrementalnih senzorjev v posameznih osi.
2. p velikosti 3x1. Predstavlja poloˇzaj vrha haptiˇcne naprave v karteziˇcnih koordinatah. Vrednosti so v mm.
3. pd velikosti 3x1. Predstavlja hitrost gibanja vrha robota
v karteziˇcnih koordinatah v enoti mm/s.
4. T velikosti 16x1. Predstavlja vrednosti transformacijske
matrike 0 T6 .
5. F je dimenzij 3x1. Predstavlja vrednosti trenutne sile, ki
jo naprava oddaja v enoti N .
6. q je vektor dimenzije 6x1. Opisuje vrednosti notranjih
koordinat v enoti rad.
7. But velikosti 2x1. Predstavlja stanje stikal na roˇcaju
haptiˇcne naprave.
ˇ
korak (tipiˇcno 0.01s)
pogovorno Casovni
9
Karakteristike
•
•
•
•
Opozorilo
Direct Feedthrough: Da
Stanja: Ne
Velikost vektorjev: Konstantna
Zero Crossing: Ne
Maksimalna vrednost sile ni omejena. Prekoraˇcitev maksimalne vrednosti doloˇcene za trajno obremenitev (0.88N),
lahko poˇskoduje napravo.
10
2.3 Omni Torque
Namen
Gonilnik haptiˇcne naprave Phantom Omni. Vhodni signal je
navor.
Knjiˇznica
Omni_matlab
Opis
Gonilnik za vzpostavitev povezave med hapriˇcno napravo
Phantom Omni in programskim paketom Matlab/Simulink.
Vhod je vektor navora M na glavne tri osi naprave. Vektor
je dimenzije 3x1. Maksimalna velikost je doloˇcena z parametrom vrednosti [200 350 200]. Vrednosti so v enoti mN m.
Gonilnik sestavlja 7 izhodnih signalov v naslednjem vrstnem
redu:
Raw
SensAble
p
pd
M
Phantom
Omni
T
M
Torque
q
But
Omni Torque
Parametri
okno
in
1. Raw velikosti 6x1. Predstavlja neobdelane vrednosti inkrementalnih senzorjev v posameznih osi.
2. p velikosti 3x1. Predstavlja poloˇzaj vrha haptiˇcne naprave v karteziˇcnih koordinatah. Vrednosti so v mm.
3. pd velikosti 3x1. Predstavlja hitrost gibanja vrha robota
v karteziˇcnih koordinatah v enoti mm/s.
4. T velikosti 16x1. Predstavlja vrednosti transformacijske
matrike 0 T6 .
5. M je dimenzij 3x1. Predstavlja vrednosti trenutnega navora, ki jo naprava oddaja v enoti mN m.
6. q je vektor dimenzije 6x1. Opisuje vrednosti notranjih
koordinat v enoti rad.
7. But velikosti 2x1. Predstavlja stanje stikal na roˇcaju
haptiˇcne naprave.
ˇ
korak (tipiˇcno 0.01s) in omejitev maksimalnega napogovorno Casovni
vora (tipiˇcno [200 350 200] mNm)
11
Karakteristike
•
•
•
•
Opozorilo
Direct Feedthrough: Da
Stanja: Ne
Velikost vektorjev: Konstantna
Zero Crossing: Ne
Maksimalna vrednost navora ni omejena. Uporaba navorov na
v bliˇzini maksimalnih vrednosti , lahko poˇskoduje napravo.
12
2.4 Vector to matrix
Namen
Pretvorba transformacijskega vektorja v transformacijsko matriko.
Knjiˇznica
Omni_matlab
Opis
pretvorba transformacijskega vektorja v transformacijsko matriko. Preslikava elementov je ponazorjena z:


































T(1x16) T(4x4)
Vector > Matrix
Parametri
okno
in
t1
t2
t3
t4
t5
t6
t7
t8
t9
t10
t11
t12
t13
t14
t15
t16






































=
t1
t2
t3
t4
t5 t9
t6 t10
t7 t11
t8 t12
t13
t14
t15
t16





ˇ
korak je podedovan po osnovnih nastavitvah simulapogovorno Casovni
cijskega koraka
13
3 Primer
Za ilustracijo delovanja gonilnika smo uporabili gonilnik Omni. Vhod v gonilnik je 3x1 vektor
sil. Predstavljen primer prikazuje enostavno regulacijo poloˇzaja vrha haptiˇcne naprave. Ob
pritisku na prvo tipko sili vrh haptiˇcne naprave proti koordinati (0,0,0). Na sliki 4 je prikazana
simulacijska shema.
RT synchro
Real −time
Synchronization
Raw
Gain
SensAble
p
−K−
Phantom
Omni
pd
MaxForce
F
F
[0 0 0 ]
Switch
Position
Scope
Scope 1
T
> WARNING <
Scope 2
Scope 3
q
Scope 4
But
Scope 5
Omni MaxForce
Selector
Scope 6
Memory 1
Memory
Polozaj (mm)
Slika 4: Simulacijska shema primera
X
Z
Stikalo
Sila (N)
Y
Cas (t)
Slika 5: Poloˇzaj vrha haptiˇcne naprave, sila na haptiˇcni napravi in stanje stikala (0-Off, 1-On)
Na sliki 5 je prikazan poloˇzaj vrha haptiˇcne naprave. Ob pritisku na stikalo se pojavi sila,
ki deluje v smeri izhodiˇscˇ ne toˇcke (0,0,0). Velikost sile je linearna glede na oddaljenost od
izhodiˇscˇ ne toˇcke (P regulator).
14
4 Izvorna koda za gonilnik: Omni
2
4
6
8
10
12
14
16
18
20
22
24
26
28
30
/*
* o m n i d r i v e r . c : M a t l a b / S i m u l i n k module f o r Phantom Ohmni
*
* −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
* −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
*
* C o p y r i g h t 1990 −2009 T a d e j P e t r i cˇ
*
* E−m a i l : t a d e j . p e t r i c @ i j s . s i
*
* I n s t i t u t ” J o zˇ e f S t e f a n ”
* $Revision : 1.0 $
*
* B u i l d w i t h : mex o m n i d r i v e r . c −l hd . l i b hdu . l i b
*
*
* −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
GPL − G e n e r a l P u b l i c L i c e n s e
*
* −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
T h i s p r o g r a m i s f r e e s o f t w a r e ; you c a n r e d i s t r i b u t e i t and / o r m o d i f y
*
i t u n d e r t h e t e r m s o f t h e GNU G e n e r a l P u b l i c L i c e n s e a s p u b l i s h e d by
*
the Free Software Foundation ; version 3 of the License .
*
*
T h i s p r o g r a m i s d i s t r i b u t e d i n t h e hope t h a t i t w i l l be u s e f u l ,
*
b u t WITHOUT ANY WARRANTY; w i t h o u t e v e n t h e i m p l i e d w a r r a n t y o f
*
MERCHANTABILITY o r FITNESS FOR A PARTICULAR PURPOSE . See t h e
*
GNU G e n e r a l P u b l i c L i c e n s e f o r more d e t a i l s .
*
*/
# d e f i n e S FUNCTION NAME o m n i d r i v e r
# d e f i n e S FUNCTION LEVEL 2
32
34
/ * Need t o i n c l u d e s i m s t r u c . h f o r t h e d e f i n i t i o n o f t h e S i m S t r u c t and
* i t s a s s o c i a t e d macro d e f i n i t i o n s . * /
# include ” simstruc . h”
36
38
40
42
44
46
/* General include */
# i n c l u d e < s t d i o . h>
# i n c l u d e < s t d l i b . h>
# i n c l u d e < a s s e r t . h>
# i n c l u d e <windows . h>
# i n c l u d e <c o n i o . h>
/ * S e n s a b l e Phantom Omni i n c l u d e * /
# i n c l u d e <HD/ hd . h>
# i n c l u d e <HDU/ h d u E r r o r . h>
# i n c l u d e <HDU/ h d u V e c t o r . h>
48
50
52
54
56
58
/ * Assumes t h e u n i t i s a PHANTOM from S e n s A b l e T e c h n o l o g i e s , I n c . * /
# d e f i n e MAX INPUT DOF
6
/ / Enkoders
# d e f i n e MAX OUTPUT DOF 3
/ / Motor ' s
/ * Maximal c o n t i n o u s f o r c e
* h d G e t D o u b l e v ( HD NOMINAL MAX CONTINUOUS FORCE , &n o m i n a l M a x C o n t i n u o u s F o r c e ) ; * /
# d e f i n e DEF nominalMaxContinuousForce 0.88
/ * G l o b a l v a r i a b l s ======================================================= * /
s t a t i c f l o a t alMotorDACValuesApp [ MAX_OUTPUT_DOF ] ;
s t a t i c f l o a t alMotorDACValuesServo [ MAX_OUTPUT_DOF ] ;
i n t buttons_y = 0 ;
60
62
/ * Number o f S−f u n c t i o n P a r a m e t e r s and m a c r o s t o a c c e s s from t h e S i m S t r u c t * /
# d e f i n e NUM PARAMS
(1)
# d e f i n e SAMPLE TIME PARAM
( ssGetSFcnParam ( S , 0) )
64
66
/ * Macros t o a c c e s s t h e S−f u n c t i o n p a r a m e t e r v a l u e s * /
# d e f i n e SAMPLE TIME
( ( r e a l T ) mxGetPr ( SAMPLE TIME PARAM ) [ 0 ] )
68
/ * F u n c t i o n : m d l C h e c k P a r a m e t e r s ========================================= * /
15
70
# d e f i n e MDL CHECK PARAMETERS
/ * Change t o # u n d e f t o remove f u n c t i o n * /
# i f d e f i n e d (MDL CHECK PARAMETERS) && d e f i n e d ( MATLAB MEX FILE )
72
74
76
78
80
82
84
86
88
90
92
94
96
98
s t a t i c v o i d mdlCheckParameters ( SimStruct * S ) {
}
# e n d i f / * MDL CHECK PARAMETERS * /
/ * F u n c t i o n : m d l I n i t i a l i z e S i z e s ===========================================
* Abstract :
The s i z e s i n f o r m a t i o n i s u s e d by S i m u l i n k t o d e t e r m i n e t h e S−f u n c t i o n
*
b l o c k ' s c h a r a c t e r i s t i c s ( number o f i n p u t s , o u t p u t s , s t a t e s , e t c . ) . * /
*
s t a t i c v o i d mdlInitializeSizes ( SimStruct * S ) {
ssSetNumSFcnParams ( S , NUM_PARAMS ) ;
i f ( ssGetNumSFcnParams ( S ) == ssGetSFcnParamsCount ( S ) ) {
#ifdef MATLAB_MEX_FILE
mdlCheckParameters ( S ) ;
#endif
i f ( ssGetErrorStatus ( S ) ! = NULL ) {
return ;
/* Error r e p o r t e d in mdlCheckParameters */
}
} else {
r e t u r n ; / * P a r a m e t e r m i s m a t c h w i l l be r e p o r t e d by S i m u l i n k * /
}
/ * None o f t h i s s−f u n c t i o n s ' s p a r a m e t e r s a r e t u n a b l e d u r i n g s i m u l a t i o n * /
{
int_T i ;
f o r ( i = 0 ; i < NUM_PARAMS ; i++) {
ssSetSFcnParamNotTunable ( S , i ) ;
}
}
100
/ * I n p u t s e t i n g s ==================================================== * /
ssSetNumSampleTimes ( S , 1 ) ;
ssSetNumInputPorts ( S , 1 ) ;
ssSetInputPortWidth ( S , 0 , 3 ) ;
ssSetInputPortDirectFeedThrough ( S , 0 , 1 ) ;
/ * O u t p u t s e t i n g s =================================================== * /
ssSetNumOutputPorts ( S , 7 ) ;
ssSetOutputPortWidth ( S , 0 , 6 ) ;
/ / Raw − e n k o d e r j i
ssSetOutputPortWidth ( S , 1 , 3 ) ;
/ / C a r t a s i a n p o s i z i o n xyz
ssSetOutputPortWidth ( S , 2 , 3 ) ;
/ / Velocity
ssSetOutputPortWidth ( S , 3 , 1 6 ) ; / / T r a n s f o r m
ssSetOutputPortWidth ( S , 4 , 3 ) ;
/ / Force
ssSetOutputPortWidth ( S , 5 , 6 ) ; / / J o i n t ' s q
ssSetOutputPortWidth ( S , 6 , 2 ) ; / / B u t t o n ' s
ssSetNumIWork ( S , 0 ) ;
ssSetNumRWork ( S , 0 ) ;
ssSetNumPWork ( S , 0 ) ;
102
104
106
108
110
112
114
116
118
}
120
/ * F u n c t i o n : m d l I n i t i a l i z e S a m p l e T i m e s =====================================
* Abstract :
This f u n c t i o n i s used to s p e c i f y t h e sample time ( s ) f o r your
*
S−f u n c t i o n . You must r e g i s t e r t h e same number o f s a m p l e t i m e s a s
*
s p e c i f i e d i n ssSetNumSampleTimes .
*
*/
s t a t i c v o i d mdlInitializeSampleTimes ( SimStruct * S ) {
ssSetSampleTime ( S , 0 , SAMPLE_TIME ) ;
ssSetOffsetTime ( S , 0 , 0 . 0 ) ;
}
122
124
126
128
130
132
134
136
138
140
# u n d e f MDL INITIALIZE CONDITIONS
/ * Change t o # u n d e f t o remove f u n c t i o n * /
/ / # d e f i n e MDL INITIALIZE CONDITIONS
/ * Change t o # u n d e f t o remove f u n c t i o n * /
# i f d e f i n e d ( MDL INITIALIZE CONDITIONS )
/ * F u n c t i o n : m d l I n i t i a l i z e C o n d i t i o n s ======================================
* Abstract :
I n t h i s f u n c t i o n , you s h o u l d i n i t i a l i z e t h e c o n t i n u o u s and d i s c r e t e
*
s t a t e s f o r y o u r S−f u n c t i o n b l o c k . The i n i t i a l s t a t e s a r e p l a c e d
*
in the s t a t e vector , ssGetContStates (S) or ssGetRealDiscStates (S) .
*
You c a n a l s o p e r f o r m any o t h e r i n i t i a l i z a t i o n a c t i v i t i e s t h a t y o u r
*
S−f u n c t i o n may r e q u i r e . Note , t h i s r o u t i n e w i l l be c a l l e d a t t h e
*
s t a r t o f s i m u l a t i o n and i f i t i s p r e s e n t i n an e n a b l e d s u b s y s t e m
*
16
142
144
146
148
150
*
*
*/
static
}
# endif
c o n f i g u r e d t o r e s e t s t a t e s , i t w i l l be c a l l when t h e e n a b l e d s u b s y s t e m
r e s t a r t s execution to r e s e t the s t a t e s .
v o i d mdlInitializeConditions ( SimStruct * S ) {
/ * MDL INITIALIZE CONDITIONS * /
/ * F u n c t i o n s f o r Phantom ================================================ * /
HDSchedulerHandle gCallbackHandle = HD_INVALID_HANDLE ;
152
154
156
158
160
162
164
166
168
170
172
174
176
178
180
182
184
186
188
190
192
194
/ * *****************************************************************
* D i r e c t l y s e t s t h e DAC v a l u e s .
***************************************************************** * /
HDCallbackCode HDCALLBACK ServoSchedulerCallback ( v o i d * pUserData ) {
HDErrorInfo error ;
hdBeginFrame ( hdGetCurrentDevice ( ) ) ;
assert ( alMotorDACValuesServo ) ;
hdSetFloatv ( HD_CURRENT_FORCE , alMotorDACValuesServo ) ;
/ / Force !
hdEndFrame ( hdGetCurrentDevice ( ) ) ;
i f ( HD_DEVICE_ERROR ( error = hdGetError ( ) ) ) {
hduPrintError ( stderr , &error , ” E r r o r w h i l e commanding DAC v a l u e s ” ) ;
memset ( alMotorDACValuesServo , 0 , s i z e o f ( f l o a t ) * MAX_OUTPUT_DOF ) ;
i f ( hduIsSchedulerError(&error ) ) {
r e t u r n HD_CALLBACK_DONE ;
}
}
r e t u r n HD_CALLBACK_CONTINUE ;
}
/ * *****************************************************************
* C o p i e s s t a t e i n t h r e a d −s a f e manner .
****************************************************************** * /
HDCallbackCode HDCALLBACK UpdateMotorDACValuesCallback ( v o i d * pUserData ) {
memcpy ( alMotorDACValuesServo , alMotorDACValuesApp , s i z e o f ( f l o a t ) * MAX_OUTPUT_DOF ) ;
r e t u r n HD_CALLBACK_DONE ;
}
/ * *****************************************************************
* Synchronization structure .
* **************************************************************** * /
typedef s t r u c t {
/ * Raw v a l u e s * /
HDlong encoder_values [ MAX_INPUT_DOF ] ;
/* Cartesian space values */
d o u b l e position [ 3 ] ;
d o u b l e velocity [ 3 ] ;
d o u b l e transform [ 1 6 ] ;
HDfloat force_xyz [ MAX_OUTPUT_DOF ] ;
/* J o i n t space values */
d o u b l e jointAngles [ 3 ] ;
d o u b l e gimbalAngles [ 3 ] ;
} DeviceStateStruct ;
196
198
200
202
204
206
208
210
212
214
/ * *******************************************************************
* Callback tha t r e t r i e v e s s t a t e .
******************************************************************** * /
HDCallbackCode HDCALLBACK GetDeviceStateCallback ( v o i d * pUserData ) {
DeviceStateStruct * pState = ( DeviceStateStruct * ) pUserData ;
/ * Raw v a l u e s * /
hdGetLongv ( HD_CURRENT_ENCODER_VALUES , pState−>encoder_values ) ;
/* Cartesian space values */
hdGetDoublev ( HD_CURRENT_POSITION , pState−>position ) ;
hdGetDoublev ( HD_CURRENT_VELOCITY , pState−>velocity ) ;
hdGetDoublev ( HD_CURRENT_TRANSFORM , pState−>transform ) ;
hdGetFloatv ( HD_CURRENT_FORCE , pState−>force_xyz ) ;
/* J o i n t space values */
hdGetDoublev ( HD_CURRENT_JOINT_ANGLES , pState−>jointAngles ) ;
hdGetDoublev ( HD_CURRENT_GIMBAL_ANGLES , pState−>gimbalAngles ) ;
buttons_y = buttons ( ) ;
r e t u r n HD_CALLBACK_DONE ;
}
17
216
218
220
222
/ * *******************************************************************
* Callback t h a t r e t r i e v e s s t a t e of buttons .
******************************************************************** * /
i n t buttons ( ) {
i n t buttons ;
hdGetIntegerv ( HD_CURRENT_BUTTONS , &buttons ) ;
r e t u r n buttons ;
}
224
226
228
230
232
234
236
# d e f i n e MDL START / * Change t o # u n d e f t o remove f u n c t i o n * /
# i f d e f i n e d (MDL START )
/ * F u n c t i o n : m d l S t a r t ==================================================
* Abstract :
T h i s f u n c t i o n i s c a l l e d o n c e a t s t a r t o f model e x e c u t i o n . I f you
*
h a v e s t a t e s t h a t s h o u l d be i n i t i a l i z e d once , t h i s i s t h e p l a c e
*
t o do i t .
*
*/
s t a t i c v o i d mdlStart ( SimStruct * S ) {
HDErrorInfo error ;
HDstring model ;
HDboolean bDone = FALSE ;
238
240
242
244
246
248
250
252
254
256
258
260
262
264
266
HHD hHD = hdInitDevice ( HD_DEFAULT_DEVICE ) ;
i f ( HD_DEVICE_ERROR ( error = hdGetError ( ) ) ) {
ssSetErrorStatus ( S , ” F a i l e d t o i n i t i a l i z e h a p t i c d e v i c e ” ) ;
return ;
}
model = hdGetString ( HD_DEVICE_MODEL_TYPE ) ;
printf ( ” I n i t i a l i z e d : %s \n ” , model ) ;
/* Schedule the h a p t i c callback function for continuously monitoring the
* b u t t o n s t a t e and r e n d e r i n g t h e a n c h o r e d s p r i n g f o r c e * /
gCallbackHandle = hdScheduleAsynchronous (
ServoSchedulerCallback , 0 , HD_MAX_SCHEDULER_PRIORITY ) ;
i f ( HD_DEVICE_ERROR ( error = hdGetError ( ) ) ) {
ssSetErrorStatus ( S , ” F a i l e d t o s c h e d u l e s e r v o l o o p c a l l b a c k ” ) ;
printf ( ” F a i l e d t o s c h e d u l e s e r v o l o o p c a l l b a c k ” ) ;
hdDisableDevice ( hHD ) ;
return ;
}
hdEnable ( HD_FORCE_OUTPUT ) ;
/* S t a r t the h a p t i c rendering loop */
hdStartScheduler ( ) ;
i f ( HD_DEVICE_ERROR ( error = hdGetError ( ) ) ) {
ssSetErrorStatus ( S , ” F a i l e d t o s t a r t s e r v o l o o p ” ) ;
printf ( ” F a i l e d t o s t a r t s e r v o l o o p ” ) ;
hdDisableDevice ( hHD ) ;
return ;
}
}
# endif /*
MDL START * /
268
270
272
274
276
278
280
282
284
286
/ * F u n c t i o n : m d l O u t p u t s ===================================================
* Abstract :
I n t h i s f u n c t i o n , you compute t h e o u t p u t s o f y o u r S−f u n c t i o n
*
block .
*
*/
s t a t i c v o i d mdlOutputs ( SimStruct * S , int_T tid ) {
int i;
HDfloat nValue [ 3 ] ;
DeviceStateStruct state ;
real_T
* y_raw = ssGetOutputPortSignal ( S , 0 ) ;
real_T
* y_position = ssGetOutputPortSignal ( S , 1 ) ;
real_T
* y_velocity = ssGetOutputPortSignal ( S , 2 ) ;
real_T
* y_transform = ssGetOutputPortSignal ( S , 3 ) ;
real_T
* y_force_xyz = ssGetOutputPortSignal ( S , 4 ) ;
real_T
* y_joint_angle = ssGetOutputPortSignal ( S , 5 ) ;
real_T
* y_butt = ssGetOutputPortSignal ( S , 6 ) ;
InputRealPtrsType u = ssGetInputPortRealSignalPtrs ( S , 0 ) ;
memset(&state , 0 , s i z e o f ( DeviceStateStruct ) ) ;
/ * Hap tic c a l l back s c h e d u l e r * /
18
288
hdScheduleSynchronous ( GetDeviceStateCallback , &state , HD_DEFAULT_SCHEDULER_PRIORITY ) ;
/ * Raw * /
f o r ( i = 0 ; i<6; i++) {
y_raw [ i ] = state . encoder_values [ i ] ;
}
/ * C a r t e s i a n s p a c e and j o i t a n g l e s * /
f o r ( i = 0 ; i<3; i++) {
y_position [ i ] = state . position [ i ] ;
y_velocity [ i ] = state . velocity [ i ] ;
y_force_xyz [ i ] = state . force_xyz [ i ] ;
y_joint_angle [ i ] = state . jointAngles [ i ] ;
y_joint_angle [ i + 3 ] = state . gimbalAngles [ i ] ;
}
/ * T r a n s f o r m a t i o n a l m a t r i x 4 x4 * /
f o r ( i = 0 ; i<16 ; i++) {
y_transform [ i ] = state . transform [ i ] ;
}
/* Button s t a t e s */
y_butt [ 0 ] = buttons_y & HD_DEVICE_BUTTON_1 ;
y_butt [ 1 ] = buttons_y & HD_DEVICE_BUTTON_2 ;
/ * L i m i t f o r c e c a l c u l a t e d t o Max c o n t i n u o u i s . T h i s i s v e r y i m p o r t a n t
* b e c a u s e f o r c e v a l u e s e x c e e d i n g t h i s v a l u e c a n damage t h e d e v i c e
* motors . */
f o r ( i = 0 ; i < 3 ; i++) {
nValue [ i ] = * u [ i ] ;
i f ( nValue [ i]> DEF_nominalMaxContinuousForce ) {
nValue [ i ] = 0 . 8 8 ; }
i f ( nValue [ i]< −DEF_nominalMaxContinuousForce ) {
nValue [ i ] = −0.88;}
alMotorDACValuesApp [ i ] = nValue [ i ] ;
}
/* Force update */
hdScheduleSynchronous ( UpdateMotorDACValuesCallback , 0 , HD_DEFAULT_SCHEDULER_PRIORITY ) ;
290
292
294
296
298
300
302
304
306
308
310
312
314
316
318
320
}
322
324
326
328
330
332
334
336
338
340
342
# u n d e f MDL UPDATE / * Change t o # u n d e f t o remove f u n c t i o n * /
# i f d e f i n e d (MDL UPDATE)
s t a t i c v o i d mdlUpdate ( SimStruct * S , int_T tid ) { }
# e n d i f / * MDL UPDATE * /
# u n d e f MDL DERIVATIVES / * Change t o # u n d e f t o remove f u n c t i o n * /
# i f d e f i n e d ( MDL DERIVATIVES )
s t a t i c v o i d mdlDerivatives ( SimStruct * S ) {}
# e n d i f / * MDL DERIVATIVES * /
/ * F u n c t i o n : m d l T e r m i n a t e =====================================================
* Abstract :
I n t h i s f u n c t i o n , you s h o u l d p e r f o r m any a c t i o n s t h a t a r e n e c e s s a r y
*
a t t h e t e r m i n a t i o n o f a s i m u l a t i o n . F o r example , i f memory was
*
a l l o c a t e d in mdlStart , t h i s i s the place to free i t .
*
*/
s t a t i c v o i d mdlTerminate ( SimStruct * S ) {
hdStopScheduler ( ) ;
hdUnschedule ( gCallbackHandle ) ;
hdDisableDevice ( hdGetCurrentDevice ( ) ) ;
}
344
346
348
350
352
/ * ============================= *
* R e q u i r e d S−f u n c t i o n t r a i l e r *
* ============================= * /
# i f d e f MATLAB MEX FILE
# include ” simulink . c”
#else
# include ” cg sfun . h”
# endif
/ * I s t h i s f i l e b e i n g c o m p i l e d a s a MEX−f i l e ? * /
/ * MEX− f i l e i n t e r f a c e mechanism * /
/ * Code g e n e r a t i o n r e g i s t r a t i o n f u n c t i o n * /
19