Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F105150043
SmaractHub.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Sat, Mar 15, 00:21
Size
96 KB
Mime Type
text/x-c
Expires
Mon, Mar 17, 00:21 (2 d)
Engine
blob
Format
Raw Data
Handle
24933506
Attached To
R10714 uSurgery main dll file
SmaractHub.cpp
View Options
This document is not UTF8. It was detected as Shift JIS and converted to UTF8 for display.
#include "SmaractHub.h"
#include "ModuleInterface.h"
#include <cstring>
#include <iostream>
#include <fstream>
#include <ctime>
#include <boost/algorithm/string.hpp>
#include <Windows.h>
#define _USE_MATH_DEFINES
#include <math.h>
#define PROPERTY_MOVE_UPPER_BOUND 1
#define PROPERTY_MOVE_LOWER_BOUND -1
#define PROPERTY_MOVE_DEFAULT "0"
#define PROPERTY_TOGGLE_DEFAULT "0"
#define PROPERTY_TOGGLE_ON "1"
#define PROPERTY_TOGGLE_OFF "0"
#define PROPERTY_SPEED_LAYER_SELECTOR_DEFAULT "1"
#define PROPERTY_SPEED_LAYER_SELECTOR_MIN "0"
#define PROPERTY_SPEED_LAYER_SELECTOR_MAX "3"
#define PROPERTY_SPEED_LAYER_DEFAULT "1000"
#define PROPERTY_SPEED_LAYER_MAX 1000
#define PROPERTY_SPEED_LAYER_MIN 1
#define PROPERTY_SPEED_FACTOR_DEFAULT "1"
#define PROPERTY_SPEED_FACTOR_MIN 0.1
#define PROPERTY_SPEED_FACTOR_MAX 10
#define PROPERTY_TIMEOUTOFFSET_DEFAULT "40"
#define PROPERTY_TIMEOUTOFFSET_MIN 0
#define PROPERTY_TIMEOUTOFFSET_MAX 1000
#define PROPERTY_TREMORFACTOR_DEFAULT "0.05"
#define PROPERTY_TREMORFACTOR_MIN 0
#define PROPERTY_TREMORFACTOR_MAX 0.7
#define PROPERTY_REPLAY_OFFSET_DEFAULT "0"
#define PROPERTY_REPLAY_OFFSET_MIN -10000
#define PROPERTY_REPLAY_OFFSET_MAX 10000
#define PACKET_TIMEOUT 10
#define REFERENCE_TIMEOUT 7000
//Timeouts are in ms
#define MAX_REPLAY_TIMEOUT 2000
#define MAX_REPLAY_SPEED 12000000
#define OLMOVE_VECT_INVERT -1
#define MICRO_TO_NANO 1000
#define ROTATION_SCALE_SHIFT -55000000
#define INVERTED_SCALE_FALSE 0
#define INVERTED_SCALE_TRUE 1
#define FILENAME_MAX_SIZE 100
#define XROT_CHANNEL 3
#define YROT_CHANNEL 4
const
char
*
g_DeviceNameSmaract6DOFHub
=
"Smaract6DOF-Hub"
;
const
char
*
g_DeviceNameSmaractX
=
"SLC2460dle-2"
;
const
char
*
g_DeviceNameSmaractY
=
"SLC2460wodle-1"
;
const
char
*
g_DeviceNameSmaractZ
=
"SLC2460dle-1"
;
const
char
*
g_DeviceNameSmaractAlpha
=
"SR4513ds-15"
;
const
char
*
g_DeviceNameSmaractBeta
=
"SR2812s"
;
const
char
*
g_DeviceNameSmaractGamma
=
"SR-1908"
;
const
char
*
g_DeviceNameToolActuator
=
"ToolActuator"
;
// Active hold time of actuator
const
unsigned
int
holdTime
=
0
;
// Parameters for open loop motion (of actuator without sensor)
const
unsigned
int
olAmplitude
=
4095
;
const
unsigned
int
olFrequency
=
18500
;
const
int
olMaxSteps
=
30000
;
// Max according to manual
// Parameters for closed loop motion
const
int
clvMax
=
100000000
;
// 100M [nm/sec] resp. [uー]
const
int
minTravelLinear
=
1000000
;
// [nm] = 1[mm]
const
int
maxTravelLinear
=
40000000
;
// [nm] = 40[mm]
const
int
minTravelRotational
=
5000000
;
//[uー] = 5ー
const
int
maxTravelRotational
=
95000000
;
//[uー] = 95ー
const
int
dSafe_nm
=
100
;
// [nm] safety margin for limits in linear motion
// Static lock
MMThreadLock
SmaractHub
::
lock_
;
/*
* Exported MMDevice API
*/
MODULE_API
void
InitializeModuleData
()
{
RegisterDevice
(
g_DeviceNameSmaract6DOFHub
,
MM
::
HubDevice
,
"Hub"
);
RegisterDevice
(
g_DeviceNameSmaractX
,
MM
::
StageDevice
,
"X linear Stage"
);
RegisterDevice
(
g_DeviceNameSmaractY
,
MM
::
StageDevice
,
"Y linear Stage"
);
RegisterDevice
(
g_DeviceNameSmaractZ
,
MM
::
StageDevice
,
"Z linear Stage"
);
RegisterDevice
(
g_DeviceNameSmaractAlpha
,
MM
::
StageDevice
,
"Alpha Rotary Stage"
);
RegisterDevice
(
g_DeviceNameSmaractBeta
,
MM
::
StageDevice
,
"Beta Rotary Stage"
);
RegisterDevice
(
g_DeviceNameSmaractGamma
,
MM
::
StageDevice
,
"Gamma Rotary Stage"
);
RegisterDevice
(
g_DeviceNameToolActuator
,
MM
::
StageDevice
,
"Arduino Stepper Tool Actuator"
);
}
MODULE_API
MM
::
Device
*
CreateDevice
(
const
char
*
deviceName
)
{
if
(
deviceName
==
0
)
return
0
;
if
(
strcmp
(
deviceName
,
g_DeviceNameSmaract6DOFHub
)
==
0
)
{
return
new
SmaractHub
;
}
else
if
(
strcmp
(
deviceName
,
g_DeviceNameSmaractX
)
==
0
)
{
return
new
SmaractTrStage
(
deviceName
);
}
else
if
(
strcmp
(
deviceName
,
g_DeviceNameSmaractY
)
==
0
)
{
return
new
SmaractTrStage
(
deviceName
);
}
else
if
(
strcmp
(
deviceName
,
g_DeviceNameSmaractZ
)
==
0
)
{
return
new
SmaractTrStage
(
deviceName
);
}
else
if
(
strcmp
(
deviceName
,
g_DeviceNameSmaractAlpha
)
==
0
)
{
return
new
SmaractRotStage
(
deviceName
);
}
else
if
(
strcmp
(
deviceName
,
g_DeviceNameSmaractBeta
)
==
0
)
{
return
new
SmaractRotStage
(
deviceName
);
}
else
if
(
strcmp
(
deviceName
,
g_DeviceNameSmaractGamma
)
==
0
)
{
return
new
SmaractRotStage
(
deviceName
);
}
else
if
(
strcmp
(
deviceName
,
g_DeviceNameToolActuator
)
==
0
)
{
return
new
ToolActuator
;
}
else
{
return
0
;
}
}
MODULE_API
void
DeleteDevice
(
MM
::
Device
*
pDevice
)
{
delete
pDevice
;
}
/*
* Smaract Hub implementation
*/
SmaractHub
::
SmaractHub
()
:
initialized_
(
false
),
isConnected_
(
false
),
newFile
(
false
),
noChannels_
(
0
),
speedFactor
(
1.0
),
prevXVect
(
0
),
prevYVect
(
0
),
prevZVect
(
0
),
tremorFactor
(
0.05
),
name_
(
g_DeviceNameSmaract6DOFHub
),
async_
(
true
),
calibrate_
(
false
),
reference_
(
false
),
hasRecordedOnce_
(
false
)
{
InitializeDefaultErrorMessages
();
SetErrorText
(
ERR_HUB_NOT_CONNECTED
,
g_Msg_ERR_HUB_NOT_CONNECTED
);
SetErrorText
(
ERR_HUB_NONE_DETECTED
,
g_Msg_ERR_HUB_NONE_DETECTED
);
SetErrorText
(
ERR_ASIGP_BAD_MULT
,
g_Msg_ERR_ASIGP_BAD_MULT
);
for
(
SA_STATUS
i
=
0
;
i
<
256
;
i
++
)
{
const
char
*
info
;
SA_GetStatusInfo
(
i
,
&
info
);
SetErrorText
(
i
,
&
info
[
0
]);
}
int
ret
=
CreateProperty
(
"usb_locator"
,
"USB locator"
,
MM
::
String
,
false
);
assert
(
DEVICE_OK
==
ret
);
CPropertyAction
*
pActOnCommMode
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnCommMode
);
CreateProperty
(
"Communication mode"
,
"Asynchronous"
,
MM
::
String
,
false
,
pActOnCommMode
,
true
);
AddAllowedValue
(
"Communication mode"
,
"Asynchronous"
);
AddAllowedValue
(
"Communication mode"
,
"Synchronous"
);
CPropertyAction
*
pActOnCalibrate
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnCalibrate
);
CreateProperty
(
"Calibration"
,
"omit"
,
MM
::
String
,
false
,
pActOnCalibrate
,
true
);
AddAllowedValue
(
"Calibration"
,
"omit"
);
AddAllowedValue
(
"Calibration"
,
"perform"
);
CPropertyAction
*
pActOnReference
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnReference
);
CreateProperty
(
"Referencing"
,
"omit"
,
MM
::
String
,
false
,
pActOnReference
,
true
);
AddAllowedValue
(
"Referencing"
,
"omit"
);
AddAllowedValue
(
"Referencing"
,
"perform"
);
}
SmaractHub
::~
SmaractHub
()
{
Shutdown
();
}
int
SmaractHub
::
Initialize
()
{
if
(
initialized_
==
false
)
{
MMThreadGuard
myLock
(
lock_
);
char
devDetected
[
4096
];
unsigned
int
bufferSize
=
sizeof
(
devDetected
);
SA_STATUS
st
=
SA_FindSystems
(
""
,
devDetected
,
&
bufferSize
);
if
(
st
!=
SA_OK
)
return
st
;
char
*
token
=
std
::
strtok
(
devDetected
,
"
\n
"
);
while
(
token
!=
NULL
)
{
devConn_
.
push_back
(
token
);
token
=
std
::
strtok
(
NULL
,
" "
);
}
if
(
devConn_
.
empty
())
return
ERR_HUB_NONE_DETECTED
;
if
(
async_
)
st
=
SA_OpenSystem
(
&
mcsHandle_
,
devConn_
.
front
().
c_str
(),
"async"
);
else
st
=
SA_OpenSystem
(
&
mcsHandle_
,
devConn_
.
front
().
c_str
(),
"sync"
);
if
(
st
!=
SA_OK
)
return
st
;
isConnected_
=
true
;
char
buf
[
4096
];
unsigned
int
bufSize
=
sizeof
(
buf
);
st
=
SA_GetSystemLocator
(
mcsHandle_
,
buf
,
&
bufSize
);
if
(
st
!=
SA_OK
)
return
st
;
SetProperty
(
"usb_locator"
,
buf
);
unsigned
int
sensorEn
;
int
alphaknown
=
0
;
int
betaknown
=
0
;
int
xknown
=
0
;
int
yknown
=
0
;
int
zknown
=
0
;
if
(
async_
)
{
st
=
SA_GetSensorEnabled_A
(
mcsHandle_
);
SA_PACKET
packet
;
st
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
)
{
return
st
;
}
else
{
if
(
packet
.
packetType
==
SA_SENSOR_ENABLED_PACKET_TYPE
)
{
unsigned
int
mode
=
packet
.
data1
;
if
(
mode
!=
SA_SENSOR_ENABLED
)
{
st
=
SA_SetSensorEnabled_A
(
mcsHandle_
,
SA_SENSOR_ENABLED
);
if
(
st
!=
SA_OK
)
{
return
st
;
}
}
}
else
return
ERR_UNEXPECTED_PACKET
;
}
//CHECK FOR PHYSICAL POSITION KNOWLEDGE OF THE STAGES
//ALPHA
st
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SR4513ds-15"
));
if
(
st
!=
SA_OK
)
return
SA_OTHER_ERROR
;
st
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
==
1
)
//Physical position is known
alphaknown
=
1
;
else
alphaknown
=
0
;
//BETA
st
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SR2812s"
));
if
(
st
!=
SA_OK
)
return
SA_OTHER_ERROR
;
st
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
==
1
)
//Physical position is known
betaknown
=
1
;
else
betaknown
=
0
;
//X
st
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-2"
));
if
(
st
!=
SA_OK
)
return
SA_OTHER_ERROR
;
st
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
==
1
)
//Physical position is known
xknown
=
1
;
else
xknown
=
0
;
//Y
st
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SLC2460wodle-1"
));
if
(
st
!=
SA_OK
)
return
SA_OTHER_ERROR
;
st
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
==
1
)
//Physical position is known
yknown
=
1
;
else
yknown
=
0
;
//Z
st
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-1"
));
if
(
st
!=
SA_OK
)
return
SA_OTHER_ERROR
;
st
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
==
1
)
//Physical position is known
zknown
=
1
;
else
zknown
=
0
;
// Final double check
if
(
alphaknown
&&
betaknown
&&
xknown
&&
yknown
&&
zknown
)
{
reference_
=
true
;
ChangeScale
();
}
else
reference_
=
false
;
}
else
{
st
=
SA_GetSensorEnabled_S
(
mcsHandle_
,
&
sensorEn
);
if
(
st
!=
SA_OK
)
return
st
;
if
(
sensorEn
!=
SA_SENSOR_ENABLED
)
{
st
=
SA_SetSensorEnabled_S
(
mcsHandle_
,
SA_SENSOR_ENABLED
);
if
(
st
!=
SA_OK
)
return
st
;
}
}
st
=
SA_GetNumberOfChannels
(
mcsHandle_
,
&
noChannels_
);
if
(
st
!=
SA_OK
)
return
st
;
int
tempspeedlayersvalue
[]
=
{
1
,
10
,
100
,
1000
};
std
::
copy
(
tempspeedlayersvalue
,
tempspeedlayersvalue
+
4
,
speedLayersValue
);
initialized_
=
true
;
CPropertyAction
*
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
Reference
);
CreateProperty
(
"externalReferencing"
,
PROPERTY_TOGGLE_DEFAULT
,
MM
::
Integer
,
false
,
pAct
);
AddAllowedValue
(
"externalReferencing"
,
PROPERTY_TOGGLE_ON
);
AddAllowedValue
(
"externalReferencing"
,
PROPERTY_TOGGLE_OFF
);
UpdateProperty
(
"externalReferencing"
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnXOLMove
);
CreateProperty
(
"XOLMove"
,
PROPERTY_MOVE_DEFAULT
,
MM
::
Float
,
false
,
pAct
);
SetPropertyLimits
(
"XOLMove"
,
PROPERTY_MOVE_LOWER_BOUND
,
PROPERTY_MOVE_UPPER_BOUND
);
UpdateProperty
(
"XOLMove"
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnYOLMove
);
CreateProperty
(
"YOLMove"
,
PROPERTY_MOVE_DEFAULT
,
MM
::
Float
,
false
,
pAct
);
SetPropertyLimits
(
"YOLMove"
,
PROPERTY_MOVE_LOWER_BOUND
,
PROPERTY_MOVE_UPPER_BOUND
);
UpdateProperty
(
"YOLMove"
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnZOLMove
);
CreateProperty
(
"ZOLMove"
,
PROPERTY_MOVE_DEFAULT
,
MM
::
Float
,
false
,
pAct
);
SetPropertyLimits
(
"ZOLMove"
,
PROPERTY_MOVE_LOWER_BOUND
,
PROPERTY_MOVE_UPPER_BOUND
);
UpdateProperty
(
"ZOLMove"
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnToolMove
);
CreateProperty
(
"ToolActuate"
,
PROPERTY_MOVE_DEFAULT
,
MM
::
Integer
,
false
,
pAct
);
SetPropertyLimits
(
"ToolActuate"
,
PROPERTY_MOVE_LOWER_BOUND
,
PROPERTY_MOVE_UPPER_BOUND
);
UpdateProperty
(
"ToolActuate"
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnRotationToggle
);
CreateProperty
(
"isRotOn"
,
PROPERTY_TOGGLE_DEFAULT
,
MM
::
Integer
,
false
,
pAct
);
AddAllowedValue
(
"isRotOn"
,
PROPERTY_TOGGLE_ON
);
AddAllowedValue
(
"isRotOn"
,
PROPERTY_TOGGLE_OFF
);
UpdateProperty
(
"isRotOn"
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnSpeedLayerChange
);
CreateProperty
(
"speedLayer"
,
PROPERTY_SPEED_LAYER_SELECTOR_DEFAULT
,
MM
::
Integer
,
false
,
pAct
);
CreateProperty
(
"recordingFile"
,
" "
,
MM
::
String
,
false
,
0
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnRecordingToggle
);
CreateProperty
(
"Recording"
,
PROPERTY_TOGGLE_DEFAULT
,
MM
::
Integer
,
false
,
pAct
);
AddAllowedValue
(
"Recording"
,
PROPERTY_TOGGLE_OFF
);
AddAllowedValue
(
"Recording"
,
PROPERTY_TOGGLE_ON
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
PlayRecording
);
CreateProperty
(
"PlayRecording"
,
PROPERTY_TOGGLE_DEFAULT
,
MM
::
Integer
,
false
,
pAct
);
AddAllowedValue
(
"PlayRecording"
,
PROPERTY_TOGGLE_OFF
);
AddAllowedValue
(
"PlayRecording"
,
PROPERTY_TOGGLE_ON
);
UpdateProperty
(
"PlayRecording"
);
CreateProperty
(
"PlayFileVerbose"
,
PROPERTY_TOGGLE_DEFAULT
,
MM
::
Integer
,
false
,
0
);
AddAllowedValue
(
"PlayFileVerbose"
,
PROPERTY_TOGGLE_OFF
);
AddAllowedValue
(
"PlayFileVerbose"
,
PROPERTY_TOGGLE_ON
);
UpdateProperty
(
"PlayFileVerbose"
);
CreateProperty
(
"speedFactor"
,
PROPERTY_SPEED_FACTOR_DEFAULT
,
MM
::
Float
,
false
,
0
);
SetPropertyLimits
(
"speedFactor"
,
PROPERTY_SPEED_FACTOR_MIN
,
PROPERTY_SPEED_FACTOR_MAX
);
CreateProperty
(
"TimeoutOffset"
,
PROPERTY_TIMEOUTOFFSET_DEFAULT
,
MM
::
Integer
,
false
,
0
);
SetPropertyLimits
(
"TimeoutOffset"
,
PROPERTY_TIMEOUTOFFSET_MIN
,
PROPERTY_TIMEOUTOFFSET_MAX
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnTremorFactorChange
);
CreateProperty
(
"TremorFactor"
,
PROPERTY_TREMORFACTOR_DEFAULT
,
MM
::
Float
,
false
,
pAct
);
SetPropertyLimits
(
"TremorFactor"
,
PROPERTY_TREMORFACTOR_MIN
,
PROPERTY_TREMORFACTOR_MAX
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractHub
::
OnSpeedLayerValueChange
);
CreateProperty
(
"SpeedLayerValue1"
,
"1"
,
MM
::
Integer
,
false
,
pAct
);
SetPropertyLimits
(
"SpeedLayerValue1"
,
PROPERTY_SPEED_LAYER_MIN
,
PROPERTY_SPEED_LAYER_MAX
);
CreateProperty
(
"SpeedLayerValue2"
,
"10"
,
MM
::
Integer
,
false
,
pAct
);
SetPropertyLimits
(
"SpeedLayerValue2"
,
PROPERTY_SPEED_LAYER_MIN
,
PROPERTY_SPEED_LAYER_MAX
);
CreateProperty
(
"SpeedLayerValue3"
,
"100"
,
MM
::
Integer
,
false
,
pAct
);
SetPropertyLimits
(
"SpeedLayerValue3"
,
PROPERTY_SPEED_LAYER_MIN
,
PROPERTY_SPEED_LAYER_MAX
);
CreateProperty
(
"SpeedLayerValue4"
,
"1000"
,
MM
::
Integer
,
false
,
pAct
);
SetPropertyLimits
(
"SpeedLayerValue4"
,
PROPERTY_SPEED_LAYER_MIN
,
PROPERTY_SPEED_LAYER_MAX
);
char
buffer
[
4
];
sprintf
(
buffer
,
"%d"
,
xknown
);
CreateProperty
(
"XIsReferenced"
,
buffer
,
MM
::
Integer
,
false
,
0
);
AddAllowedValue
(
"XIsReferenced"
,
PROPERTY_TOGGLE_OFF
);
AddAllowedValue
(
"XIsReferenced"
,
PROPERTY_TOGGLE_ON
);
sprintf
(
buffer
,
"%d"
,
yknown
);
CreateProperty
(
"YIsReferenced"
,
buffer
,
MM
::
Integer
,
false
,
0
);
AddAllowedValue
(
"YIsReferenced"
,
PROPERTY_TOGGLE_OFF
);
AddAllowedValue
(
"YIsReferenced"
,
PROPERTY_TOGGLE_ON
);
sprintf
(
buffer
,
"%d"
,
zknown
);
CreateProperty
(
"ZIsReferenced"
,
buffer
,
MM
::
Integer
,
false
,
0
);
AddAllowedValue
(
"ZIsReferenced"
,
PROPERTY_TOGGLE_OFF
);
AddAllowedValue
(
"ZIsReferenced"
,
PROPERTY_TOGGLE_ON
);
sprintf
(
buffer
,
"%d"
,
alphaknown
);
CreateProperty
(
"AlphaIsReferenced"
,
buffer
,
MM
::
Integer
,
false
,
0
);
AddAllowedValue
(
"AlphaIsReferenced"
,
PROPERTY_TOGGLE_OFF
);
AddAllowedValue
(
"AlphaIsReferenced"
,
PROPERTY_TOGGLE_ON
);
sprintf
(
buffer
,
"%d"
,
betaknown
);
CreateProperty
(
"BetaIsReferenced"
,
buffer
,
MM
::
Integer
,
false
,
0
);
AddAllowedValue
(
"BetaIsReferenced"
,
PROPERTY_TOGGLE_OFF
);
AddAllowedValue
(
"BetaIsReferenced"
,
PROPERTY_TOGGLE_ON
);
CreateProperty
(
"XOffset"
,
PROPERTY_REPLAY_OFFSET_DEFAULT
,
MM
::
Integer
,
false
,
0
);
SetPropertyLimits
(
"XOffset"
,
PROPERTY_REPLAY_OFFSET_MIN
,
PROPERTY_REPLAY_OFFSET_MAX
);
CreateProperty
(
"YOffset"
,
PROPERTY_REPLAY_OFFSET_DEFAULT
,
MM
::
Integer
,
false
,
0
);
SetPropertyLimits
(
"YOffset"
,
PROPERTY_REPLAY_OFFSET_MIN
,
PROPERTY_REPLAY_OFFSET_MAX
);
CreateProperty
(
"ZOffset"
,
PROPERTY_REPLAY_OFFSET_DEFAULT
,
MM
::
Integer
,
false
,
0
);
SetPropertyLimits
(
"ZOffset"
,
PROPERTY_REPLAY_OFFSET_MIN
,
PROPERTY_REPLAY_OFFSET_MAX
);
}
return
DEVICE_OK
;
}
void
SmaractHub
::
GetName
(
char
*
name
)
const
{
CDeviceUtils
::
CopyLimitedString
(
name
,
name_
.
c_str
());
}
bool
SmaractHub
::
Busy
()
{
return
false
;
}
int
SmaractHub
::
Shutdown
()
{
if
(
initialized_
==
true
)
{
SA_STATUS
st
=
SA_CloseSystem
(
mcsHandle_
);
if
(
st
!=
SA_OK
)
return
st
;
isConnected_
=
false
;
initialized_
=
false
;
}
//Closes the recording input file
CloseRecordingFile
();
return
DEVICE_OK
;
}
bool
SmaractHub
::
IsConnected
()
{
return
isConnected_
;
}
MM
::
DeviceDetectionStatus
SmaractHub
::
DetectDevice
(
void
)
{
MM
::
DeviceDetectionStatus
result
=
MM
::
Misconfigured
;
if
(
initialized_
==
true
)
return
MM
::
CanCommunicate
;
else
return
MM
::
CanNotCommunicate
;
}
// Currently returns DEV_OK in any configuration.
// TODO: return DEV_OK only if channel configuration is in by the code intended state
int
SmaractHub
::
DetectInstalledDevices
()
{
if
(
MM
::
CanCommunicate
==
DetectDevice
())
{
peripherals_
.
clear
();
// Detect the connected devices
for
(
unsigned
int
ch
=
0
;
ch
<
noChannels_
;
ch
++
)
{
unsigned
int
sensorType
;
if
(
async_
)
{
SA_STATUS
st
=
SA_GetSensorType_A
(
mcsHandle_
,
ch
);
if
(
st
!=
SA_OK
)
return
st
;
SA_PACKET
packet
;
st
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
)
return
st
;
else
{
if
((
packet
.
packetType
==
SA_SENSOR_TYPE_PACKET_TYPE
)
&&
(
packet
.
channelIndex
==
ch
))
sensorType
=
packet
.
data1
;
else
return
ERR_UNEXPECTED_PACKET
;
}
}
else
{
SA_STATUS
st
=
SA_GetSensorType_S
(
mcsHandle_
,
ch
,
&
sensorType
);
if
(
st
!=
SA_OK
)
return
st
;
}
if
(
sensorType
!=
SA_NO_SENSOR_TYPE
)
{
switch
(
ch
)
{
case
0
:
if
(
sensorType
==
SA_LD_SENSOR_TYPE
)
{
peripherals_
.
push_back
(
g_DeviceNameSmaractX
);
break
;
}
case
1
:
if
(
sensorType
==
SA_LD_SENSOR_TYPE
)
{
peripherals_
.
push_back
(
g_DeviceNameSmaractY
);
break
;
}
case
2
:
if
(
sensorType
==
SA_LD_SENSOR_TYPE
)
{
peripherals_
.
push_back
(
g_DeviceNameSmaractZ
);
break
;
}
case
3
:
if
(
sensorType
==
SA_SR_SENSOR_TYPE
)
{
peripherals_
.
push_back
(
g_DeviceNameSmaractAlpha
);
break
;
}
case
4
:
if
(
sensorType
==
SA_SR_SENSOR_TYPE
)
{
peripherals_
.
push_back
(
g_DeviceNameSmaractBeta
);
break
;
}
case
5
:
if
(
sensorType
==
SA_SR20_SENSOR_TYPE
)
{
// TODO bug?
peripherals_
.
push_back
(
g_DeviceNameSmaractGamma
);
break
;
}
default
:
break
;
}
}
}
for
(
std
::
vector
<
std
::
string
>::
iterator
it
=
peripherals_
.
begin
();
it
!=
peripherals_
.
end
();
it
++
)
{
MM
::
Device
*
pDev
=
::
CreateDevice
(
it
->
c_str
());
if
(
pDev
)
{
AddInstalledDevice
(
pDev
);
}
}
}
return
DEVICE_OK
;
}
unsigned
int
SmaractHub
::
getChannel
(
const
char
*
devName
)
{
if
(
strcmp
(
devName
,
g_DeviceNameSmaractX
)
==
0
)
return
0
;
else
if
(
strcmp
(
devName
,
g_DeviceNameSmaractY
)
==
0
)
return
1
;
else
if
(
strcmp
(
devName
,
g_DeviceNameSmaractZ
)
==
0
)
return
2
;
else
if
(
strcmp
(
devName
,
g_DeviceNameSmaractAlpha
)
==
0
)
return
3
;
else
if
(
strcmp
(
devName
,
g_DeviceNameSmaractBeta
)
==
0
)
return
4
;
else
if
(
strcmp
(
devName
,
g_DeviceNameSmaractGamma
)
==
0
)
return
5
;
else
return
255
;
}
SA_INDEX
SmaractHub
::
getMCSHandle
()
{
return
mcsHandle_
;
}
int
SmaractHub
::
OnCommMode
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
{
/* The controller can not report whether it is in synchronous or asynchronous mode
therefore we cache the value.*/
if
(
async_
==
true
)
pProp
->
Set
(
"Asynchronous"
);
else
pProp
->
Set
(
"Synchronous"
);
return
DEVICE_OK
;
}
else
if
(
eAct
==
MM
::
AfterSet
)
{
std
::
string
commMode
;
pProp
->
Get
(
commMode
);
if
(
commMode
==
"Asynchronous"
)
async_
=
true
;
else
async_
=
false
;
/* This change has only once an effect, i.e. upon calling initialize(),
after initialization, the communication mode cannot be changed anymore.*/
}
return
DEVICE_OK
;
}
int
SmaractHub
::
OnCalibrate
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
{
if
(
calibrate_
==
true
)
pProp
->
Set
(
"perform"
);
else
pProp
->
Set
(
"omit"
);
return
DEVICE_OK
;
}
else
if
(
eAct
==
MM
::
AfterSet
)
{
std
::
string
calibrate
;
pProp
->
Get
(
calibrate
);
if
(
calibrate
==
"perform"
)
calibrate_
=
true
;
else
calibrate_
=
false
;
}
return
DEVICE_OK
;
}
int
SmaractHub
::
OnReference
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
{
if
(
reference_
==
true
)
pProp
->
Set
(
"perform"
);
else
pProp
->
Set
(
"omit"
);
return
DEVICE_OK
;
}
else
if
(
eAct
==
MM
::
AfterSet
)
{
std
::
string
reference
;
pProp
->
Get
(
reference
);
if
(
reference
==
"perform"
)
reference_
=
true
;
else
reference_
=
false
;
}
return
DEVICE_OK
;
}
int
SmaractHub
::
OnSpeedLayerChange
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
return
SA_OK
;
long
speedLayer
;
long
recording
;
SA_STATUS
st
=
GetProperty
(
"Recording"
,
recording
);
if
(
st
!=
SA_OK
)
return
st
;
if
(
recording
)
st
=
Snapshot
();
if
(
st
!=
SA_OK
)
return
st
;
/*
pProp->Get(speedLayer);
SetTRFrequency(speedLayer*5);*/
GetCoreCallback
()
->
OnPropertiesChanged
(
this
);
return
st
;
}
int
SmaractHub
::
OnSpeedLayerValueChange
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
return
SA_OK
;
long
val1
;
long
val2
;
long
val3
;
long
val4
;
GetProperty
(
"SpeedLayerValue1"
,
val1
);
GetProperty
(
"SpeedLayerValue2"
,
val2
);
GetProperty
(
"SpeedLayerValue3"
,
val3
);
GetProperty
(
"SpeedLayerValue4"
,
val4
);
int
tempspeedlayersvalue
[]
=
{
val1
,
val2
,
val3
,
val4
};
std
::
copy
(
tempspeedlayersvalue
,
tempspeedlayersvalue
+
4
,
speedLayersValue
);
return
SA_OK
;
}
int
SmaractHub
::
OnTremorFactorChange
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
return
SA_OK
;
SA_STATUS
st
=
GetProperty
(
"TremorFactor"
,
tremorFactor
);
return
st
;
}
int
SmaractHub
::
OnRotationToggle
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
return
SA_OK
;
GetCoreCallback
()
->
OnPropertiesChanged
(
this
);
return
SA_OK
;
}
int
SmaractHub
::
OnXOLMove
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
!
reference_
)
{
/*
Throw the MM error popup that the stage is not referenced
*/
return
ERR_NOT_REFERENCED
;
}
long
rotOn
;
SA_STATUS
st
=
GetProperty
(
"isRotOn"
,
rotOn
);
if
(
st
!=
DEVICE_OK
)
return
st
;
double
vect
;
pProp
->
Get
(
vect
);
if
(
vect
==
0
)
prevXVect
=
vect
;
else
if
(
abs
(
vect
-
prevXVect
)
>
tremorFactor
)
prevXVect
=
vect
;
else
return
DEVICE_OK
;
if
(
rotOn
&&
xROTStage
)
{
pProp
->
Set
(
vect
*
OLMOVE_VECT_INVERT
);
st
=
xROTStage
->
OnOLMove
(
pProp
,
eAct
);
if
(
st
!=
DEVICE_OK
)
return
st
;
}
else
if
(
xTRStage
)
{
st
=
xTRStage
->
OnOLMove
(
pProp
,
eAct
);
if
(
st
!=
DEVICE_OK
)
return
st
;
}
else
return
ERR_DEVICE_NOT_FOUND
;
return
DEVICE_OK
;
}
int
SmaractHub
::
OnYOLMove
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
!
reference_
)
{
/*
Throw the MM error popup that the stage is not referenced
*/
return
ERR_NOT_REFERENCED
;
}
long
rotOn
;
SA_STATUS
st
=
GetProperty
(
"isRotOn"
,
rotOn
);
if
(
st
!=
DEVICE_OK
)
return
st
;
double
vect
;
pProp
->
Get
(
vect
);
if
(
vect
==
0
)
prevYVect
=
vect
;
else
if
(
abs
(
vect
-
prevYVect
)
>
tremorFactor
)
prevYVect
=
vect
;
else
return
DEVICE_OK
;
if
(
rotOn
&&
yROTStage
)
{
st
=
yROTStage
->
OnOLMove
(
pProp
,
eAct
);
if
(
st
!=
DEVICE_OK
)
return
st
;
}
else
if
(
yTRStage
)
{
st
=
yTRStage
->
OnOLMove
(
pProp
,
eAct
);
if
(
st
!=
DEVICE_OK
)
return
st
;
}
else
return
ERR_DEVICE_NOT_FOUND
;
return
DEVICE_OK
;
}
int
SmaractHub
::
OnZOLMove
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
!
reference_
)
{
/*
Throw the MM error popup that the stage is not referenced
*/
return
ERR_NOT_REFERENCED
;
}
long
rotOn
;
SA_STATUS
st
=
GetProperty
(
"isRotOn"
,
rotOn
);
if
(
st
!=
DEVICE_OK
)
return
st
;
double
vect
;
pProp
->
Get
(
vect
);
if
(
vect
==
0
)
prevZVect
=
vect
;
else
if
(
abs
(
vect
-
prevZVect
)
>
tremorFactor
)
prevZVect
=
vect
;
else
return
DEVICE_OK
;
if
(
rotOn
&&
zROTStage
)
{
st
=
zROTStage
->
OnOLMove
(
pProp
,
eAct
);
if
(
st
!=
DEVICE_OK
)
return
st
;
}
else
if
(
zTRStage
)
{
st
=
zTRStage
->
OnOLMove
(
pProp
,
eAct
);
if
(
st
!=
DEVICE_OK
)
return
st
;
}
else
return
ERR_DEVICE_NOT_FOUND
;
return
DEVICE_OK
;
}
int
SmaractHub
::
OnToolMove
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
!
reference_
)
{
/*
Throw the MM error popup that the stage is not referenced
*/
return
ERR_NOT_REFERENCED
;
}
toolActuator
->
OnMove
(
pProp
,
eAct
);
return
DEVICE_OK
;
}
bool
SmaractHub
::
isCommModeAsync
()
{
return
async_
;
}
bool
SmaractHub
::
isCalibrate
()
{
return
calibrate_
;
}
bool
SmaractHub
::
isReference
()
{
return
reference_
;
}
void
SmaractHub
::
SetTRStage
(
const
char
*
name
,
SmaractTrStage
*
pStage
)
{
if
(
strcmp
(
name
,
g_DeviceNameSmaractX
)
==
0
)
{
xTRStage
=
pStage
;
}
else
if
(
strcmp
(
name
,
g_DeviceNameSmaractY
)
==
0
)
{
yTRStage
=
pStage
;
}
else
if
(
strcmp
(
name
,
g_DeviceNameSmaractZ
)
==
0
)
{
zTRStage
=
pStage
;
}
else
return
;
}
void
SmaractHub
::
SetRotStage
(
const
char
*
name
,
SmaractRotStage
*
pStage
)
{
if
(
strcmp
(
name
,
g_DeviceNameSmaractAlpha
)
==
0
)
{
xROTStage
=
pStage
;
}
else
if
(
strcmp
(
name
,
g_DeviceNameSmaractBeta
)
==
0
)
{
yROTStage
=
pStage
;
}
else
if
(
strcmp
(
name
,
g_DeviceNameSmaractGamma
)
==
0
)
{
zROTStage
=
pStage
;
}
else
return
;
}
void
SmaractHub
::
SetToolActuator
(
ToolActuator
*
pStage
)
{
toolActuator
=
pStage
;
}
int
SmaractHub
::
OnRecordingToggle
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
return
SA_OK
;
long
recording
;
SA_STATUS
st
=
GetProperty
(
"Recording"
,
recording
);
if
(
st
!=
SA_OK
)
return
st
;
GetCoreCallback
()
->
OnPropertiesChanged
(
this
);
if
(
recording
==
1
)
OpenNewFile
();
else
CloseRecordingFile
();
return
st
;
}
/* Creates a new file in which the program will write the recording data.
The format of the file is YYYY-MM-DD-hh[h]mm[m]ss[s] */
bool
SmaractHub
::
OpenNewFile
()
{
time_t
now
=
time
(
0
);
tm
*
ltm
=
localtime
(
&
now
);
std
::
string
dateString
;
dateString
+=
std
::
to_string
(
static_cast
<
unsigned
long
long
>
(
1900
+
ltm
->
tm_year
))
+
"-"
+
std
::
to_string
(
static_cast
<
unsigned
long
long
>
(
1
+
ltm
->
tm_mon
))
+
"-"
+
std
::
to_string
(
static_cast
<
unsigned
long
long
>
(
ltm
->
tm_mday
))
+
"-"
+
std
::
to_string
(
static_cast
<
unsigned
long
long
>
(
ltm
->
tm_hour
))
+
"h"
+
std
::
to_string
(
static_cast
<
unsigned
long
long
>
(
ltm
->
tm_min
))
+
"m"
+
std
::
to_string
(
static_cast
<
unsigned
long
long
>
(
ltm
->
tm_sec
))
+
"s"
;
recordingFile
.
open
(
"Recordings/"
+
dateString
);
newFile
=
true
;
Snapshot
();
return
true
;
}
void
SmaractHub
::
CloseRecordingFile
()
{
Snapshot
();
recordingFile
.
close
();
}
/* Records the position of all stages */
int
SmaractHub
::
Snapshot
()
{
long
speedLayer
;
GetProperty
(
"speedLayer"
,
speedLayer
);
std
::
string
name
=
""
;
double
pos
;
long
toolpos
;
SA_STATUS
st
;
std
::
string
recordData
=
""
;
if
(
newFile
==
true
)
{
newFile
=
false
;
xTRStage
->
GetPositionUm
(
pos
);
name
=
"X"
;
recordData
=
name
+
" "
+
std
::
to_string
(
static_cast
<
long
long
>
(
pos
))
+
" "
;
WriteToRecordingFile
(
recordData
);
}
else
{
xTRStage
->
GetPositionUm
(
pos
);
name
=
"X"
;
recordData
=
"
\n
"
+
name
+
" "
+
std
::
to_string
(
static_cast
<
long
long
>
(
pos
))
+
" "
;
WriteToRecordingFile
(
recordData
);
}
yTRStage
->
GetPositionUm
(
pos
);
name
=
"Y"
;
recordData
=
name
+
" "
+
std
::
to_string
(
static_cast
<
long
long
>
(
pos
))
+
" "
;
WriteToRecordingFile
(
recordData
);
zTRStage
->
GetPositionUm
(
pos
);
name
=
"Z"
;
recordData
=
name
+
" "
+
std
::
to_string
(
static_cast
<
long
long
>
(
pos
))
+
" "
;
WriteToRecordingFile
(
recordData
);
xROTStage
->
GetPositionUm
(
pos
);
name
=
"Alpha"
;
recordData
=
name
+
" "
+
std
::
to_string
(
static_cast
<
long
long
>
(
pos
))
+
" "
;
WriteToRecordingFile
(
recordData
);
yROTStage
->
GetPositionUm
(
pos
);
name
=
"Beta"
;
recordData
=
name
+
" "
+
std
::
to_string
(
static_cast
<
long
long
>
(
pos
))
+
" "
;
WriteToRecordingFile
(
recordData
);
toolActuator
->
GetPositionSteps
(
toolpos
);
name
=
toolActuator
->
ExportName
();
recordData
=
name
+
" "
+
std
::
to_string
(
static_cast
<
long
long
>
(
toolpos
))
+
" "
;
WriteToRecordingFile
(
recordData
);
recordData
=
"Speedlayer "
+
std
::
to_string
(
static_cast
<
long
long
>
(
speedLayersValue
[
speedLayer
]));
WriteToRecordingFile
(
recordData
);
return
SA_OK
;
}
int
SmaractHub
::
RecordStage
(
double
pos
,
long
speedLayer
,
std
::
string
name
)
{
std
::
string
recordData
=
name
+
" "
+
std
::
to_string
(
static_cast
<
long
long
>
(
pos
))
+
" "
+
std
::
to_string
(
static_cast
<
long
long
>
(
speedLayer
));
WriteToRecordingFile
(
recordData
);
return
SA_OK
;
}
int
SmaractHub
::
RecordTool
(
long
pos
,
std
::
string
name
)
{
std
::
string
recordData
=
name
+
" "
+
std
::
to_string
(
static_cast
<
long
long
>
(
pos
));
WriteToRecordingFile
(
recordData
);
return
SA_OK
;
}
/* Takes the data string and writes it in the previously created file */
void
SmaractHub
::
WriteToRecordingFile
(
std
::
string
input
)
{
recordingFile
<<
input
;
}
/*
WARNING - THIS FUNCTION IS USED TO REFERENCE THE AXES FOR THE ROTATIONAL STAGES
DO NOT USE IT UNLESS YOU REALLY KNOW WHAT YOU'RE DOING AND CONSULTED THE
GUIDE ON HOW TO REFERENCE THE AXIS OF A STAGE CORRECTLY. THIS MAY BREAK YOUR STAGES
IF NOT DONE PROPERLY.
*/
int
SmaractHub
::
Reference
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
return
SA_OK
;
long
int
checkIsOn
=
0
;
pProp
->
Get
(
checkIsOn
);
if
(
checkIsOn
==
0
)
return
0
;
SA_PACKET
packet
;
SA_STATUS
status
;
do
{
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// This empties the packet sink
PACKET_TIMEOUT
,
&
packet
);
}
while
(
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
if
(
reference_
)
{
MessageBox
(
nullptr
,
TEXT
(
"Referencing already done"
),
TEXT
(
"Message"
),
MB_OK
|
MB_ICONINFORMATION
|
MB_SETFOREGROUND
);
return
SA_OK
;
}
else
{
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SR4513ds-15"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
!=
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
// REFERENCING THE ALPHA STAGE
status
=
SA_FindReferenceMark_A
(
mcsHandle_
,
getChannel
(
"SR4513ds-15"
),
SA_FORWARD_DIRECTION
,
0
,
SA_AUTO_ZERO
);
if
(
status
!=
SA_OK
)
return
status
;
do
{
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Waits until the packet is a SA_COMPLETED_PACKET_TYPE ( = 3)
REFERENCE_TIMEOUT
,
&
packet
);
if
(
packet
.
packetType
==
SA_NO_PACKET_TYPE
)
break
;
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
);
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SR4513ds-15"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
!=
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
MessageBox
(
nullptr
,
TEXT
(
"ERROR : Referencing has failed on alpha-Stage"
),
TEXT
(
"Error"
),
MB_OK
|
MB_ICONERROR
|
MB_SETFOREGROUND
);
throw
ERR_NOT_REFERENCED
;
}
}
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SR2812s"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
!=
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
// REFERENCING THE BETA STAGE
status
=
SA_FindReferenceMark_A
(
mcsHandle_
,
getChannel
(
"SR2812s"
),
SA_BACKWARD_DIRECTION
,
0
,
SA_AUTO_ZERO
);
if
(
status
!=
SA_OK
)
return
status
;
do
{
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Waits until the packet is a SA_COMPLETED_PACKET_TYPE ( = 3)
REFERENCE_TIMEOUT
,
&
packet
);
if
(
packet
.
packetType
==
SA_NO_PACKET_TYPE
)
break
;
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
);
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SR2812s"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
!=
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
MessageBox
(
nullptr
,
TEXT
(
"ERROR : Referencing has failed on beta-Stage"
),
TEXT
(
"Error"
),
MB_OK
|
MB_ICONERROR
|
MB_SETFOREGROUND
);
throw
ERR_NOT_REFERENCED
;
}
}
//X-Stage
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-2"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
!=
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
SA_SetSafeDirection_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-2"
),
SA_BACKWARD_DIRECTION
);
SA_CalibrateSensor_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-2"
));
do
{
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Waits until the packet is a SA_COMPLETED_PACKET_TYPE ( = 3)
REFERENCE_TIMEOUT
,
&
packet
);
if
(
packet
.
packetType
==
SA_NO_PACKET_TYPE
)
break
;
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
);
status
=
SA_FindReferenceMark_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-2"
),
SA_BACKWARD_DIRECTION
,
0
,
SA_AUTO_ZERO
);
if
(
status
!=
SA_OK
)
return
status
;
do
{
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Waits until the packet is a SA_COMPLETED_PACKET_TYPE ( = 3)
REFERENCE_TIMEOUT
,
&
packet
);
if
(
packet
.
packetType
==
SA_NO_PACKET_TYPE
)
break
;
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
);
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-2"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
!=
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
MessageBox
(
nullptr
,
TEXT
(
"ERROR : Referencing has failed on X-Stage"
),
TEXT
(
"Error"
),
MB_OK
|
MB_ICONERROR
|
MB_SETFOREGROUND
);
throw
ERR_NOT_REFERENCED
;
}
}
//Y-Stage
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SLC2460wodle-1"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
!=
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
SA_SetSafeDirection_A
(
mcsHandle_
,
getChannel
(
"SLC2460wodle-1"
),
SA_BACKWARD_DIRECTION
);
SA_CalibrateSensor_A
(
mcsHandle_
,
getChannel
(
"SLC2460wodle-1"
));
do
{
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Waits until the packet is a SA_COMPLETED_PACKET_TYPE ( = 3)
REFERENCE_TIMEOUT
,
&
packet
);
if
(
packet
.
packetType
==
SA_NO_PACKET_TYPE
)
break
;
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
);
status
=
SA_FindReferenceMark_A
(
mcsHandle_
,
getChannel
(
"SLC2460wodle-1"
),
SA_BACKWARD_DIRECTION
,
0
,
SA_AUTO_ZERO
);
if
(
status
!=
SA_OK
)
return
status
;
do
{
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Waits until the packet is a SA_COMPLETED_PACKET_TYPE ( = 3)
REFERENCE_TIMEOUT
,
&
packet
);
if
(
packet
.
packetType
==
SA_NO_PACKET_TYPE
)
break
;
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
);
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SLC2460wodle-1"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
!=
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
MessageBox
(
nullptr
,
TEXT
(
"ERROR : Referencing has failed on Y-Stage"
),
TEXT
(
"Error"
),
MB_OK
|
MB_ICONERROR
|
MB_SETFOREGROUND
);
throw
ERR_NOT_REFERENCED
;
}
}
//Z-Stage
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-1"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
!=
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
SA_SetSafeDirection_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-1"
),
SA_BACKWARD_DIRECTION
);
SA_CalibrateSensor_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-1"
));
do
{
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Waits until the packet is a SA_COMPLETED_PACKET_TYPE ( = 3)
REFERENCE_TIMEOUT
,
&
packet
);
if
(
packet
.
packetType
==
SA_NO_PACKET_TYPE
)
break
;
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
);
status
=
SA_FindReferenceMark_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-1"
),
SA_BACKWARD_DIRECTION
,
0
,
SA_AUTO_ZERO
);
if
(
status
!=
SA_OK
)
return
status
;
do
{
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Waits until the packet is a SA_COMPLETED_PACKET_TYPE ( = 3)
REFERENCE_TIMEOUT
,
&
packet
);
if
(
packet
.
packetType
==
SA_NO_PACKET_TYPE
)
break
;
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
);
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SLC2460dle-1"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
!=
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
MessageBox
(
nullptr
,
TEXT
(
"ERROR : Referencing has failed on Z-Stage"
),
TEXT
(
"Error"
),
MB_OK
|
MB_ICONERROR
|
MB_SETFOREGROUND
);
throw
ERR_NOT_REFERENCED
;
}
}
}
ChangeScale
();
MoveTRStages
();
reference_
=
true
;
SetProperty
(
"XIsReferenced"
,
"1"
);
SetProperty
(
"YIsReferenced"
,
"1"
);
SetProperty
(
"ZIsReferenced"
,
"1"
);
SetProperty
(
"AlphaIsReferenced"
,
"1"
);
SetProperty
(
"BetaIsReferenced"
,
"1"
);
GetCoreCallback
()
->
OnPropertiesChanged
(
this
);
MessageBox
(
nullptr
,
TEXT
(
"Referencing completed"
),
TEXT
(
"Message"
),
MB_OK
|
MB_ICONINFORMATION
|
MB_SETFOREGROUND
);
return
SA_OK
;
}
/*
WARNING - THIS FUNCTION IS USED TO SHIFT THE AXES FOR THE ROTATIONAL STAGES
DO NOT USE IT UNLESS YOU REALLY KNOW WHAT YOU'RE DOING AND CONSULTED THE
GUIDE ON HOW TO SHIFT THE AXIS OF A STAGE CORRECTLY
*/
int
SmaractHub
::
ChangeScale
()
{
unsigned
int
physicalIsKnown
=
0
;
SA_PACKET
packet
;
SA_STATUS
status
;
int
scaleShift
=
ROTATION_SCALE_SHIFT
;
do
{
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Empties the packet sink
PACKET_TIMEOUT
,
&
packet
);
}
while
(
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
// CHECK FOR PHYSICAL POSITION KNOWLEDGE OF STAGE 4
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SR4513ds-15"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
==
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
status
=
SA_SetScale_A
(
mcsHandle_
,
getChannel
(
"SR4513ds-15"
),
scaleShift
,
INVERTED_SCALE_FALSE
);
// Changes the scale
if
(
status
!=
SA_OK
)
return
status
;
}
// CHECK FOR PHYSICAL POSITION KNOWLEDGE OF STAGE 5
status
=
SA_GetPhysicalPositionKnown_A
(
mcsHandle_
,
getChannel
(
"SR2812s"
));
if
(
status
!=
SA_OK
)
return
status
;
status
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
//Receive the physical position knowledge packet (SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE is 13)
PACKET_TIMEOUT
,
&
packet
);
if
(
status
!=
SA_OK
||
packet
.
packetType
!=
SA_PHYSICAL_POSITION_KNOWN_PACKET_TYPE
)
// Checks the status and the packet type
return
ERR_UNEXPECTED_PACKET
;
if
(
packet
.
data1
==
SA_PHYSICAL_POSITION_KNOWN
)
//Check if physical position is known
{
status
=
SA_SetScale_A
(
mcsHandle_
,
getChannel
(
"SR2812s"
),
0
,
INVERTED_SCALE_TRUE
);
// Changes the scale
if
(
status
!=
SA_OK
)
return
status
;
}
return
SA_OK
;
}
int
SmaractHub
::
MoveTRStages
()
{
SA_PACKET
packet
;
SA_STATUS
st
;
SA_INDEX
tempChannel
;
/*
const char* g_DeviceNameSmaractX = "SLC2460dle-2";
const char* g_DeviceNameSmaractY = "SLC2460wodle-1";
const char* g_DeviceNameSmaractZ = "SLC2460dle-1";
*/
tempChannel
=
getChannel
(
"SLC2460dle-2"
);
st
=
SA_SetClosedLoopMoveSpeed_A
(
getMCSHandle
(),
tempChannel
,
(
int
)((
double
)
clvMax
*
1000
/
2000
));
if
(
st
!=
SA_OK
)
return
st
;
SA_GotoPositionAbsolute_A
(
getMCSHandle
(),
tempChannel
,
static_cast
<
signed
int
>
(
maxTravelLinear
/
2
),
holdTime
);
do
{
SA_ReceiveNextPacket_A
(
getMCSHandle
(),
// This empties the packet sink
10
,
&
packet
);
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
&&
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
tempChannel
=
getChannel
(
"SLC2460wodle-1"
);
st
=
SA_SetClosedLoopMoveSpeed_A
(
getMCSHandle
(),
tempChannel
,
(
int
)((
double
)
clvMax
*
1000
/
2000
));
if
(
st
!=
SA_OK
)
return
st
;
SA_GotoPositionAbsolute_A
(
getMCSHandle
(),
tempChannel
,
static_cast
<
signed
int
>
(
maxTravelLinear
/
2
),
holdTime
);
do
{
SA_ReceiveNextPacket_A
(
getMCSHandle
(),
// This empties the packet sink
10
,
&
packet
);
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
&&
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
tempChannel
=
getChannel
(
"SLC2460dle-1"
);
st
=
SA_SetClosedLoopMoveSpeed_A
(
getMCSHandle
(),
tempChannel
,
(
int
)((
double
)
clvMax
*
1000
/
2000
));
if
(
st
!=
SA_OK
)
return
st
;
SA_GotoPositionAbsolute_A
(
getMCSHandle
(),
tempChannel
,
static_cast
<
signed
int
>
(
maxTravelLinear
/
2
),
holdTime
);
do
{
SA_ReceiveNextPacket_A
(
getMCSHandle
(),
// This empties the packet sink
10
,
&
packet
);
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
&&
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
/*
long speedlayer;
st = GetProperty("speedLayer", speedlayer);
if (st != SA_OK)
return st;
*/
//Also move the beta stage to 5ー
tempChannel
=
getChannel
(
"SR2812s"
);
st
=
SA_SetClosedLoopMoveSpeed_A
(
getMCSHandle
(),
tempChannel
,
(
int
)((
double
)
clvMax
*
1000
/
2000
));
if
(
st
!=
SA_OK
)
return
st
;
SA_GotoAngleAbsolute_A
(
getMCSHandle
(),
tempChannel
,
static_cast
<
signed
int
>
(
minTravelRotational
),
0
,
holdTime
);
do
{
SA_ReceiveNextPacket_A
(
getMCSHandle
(),
// This empties the packet sink
10
,
&
packet
);
}
while
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
&&
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
return
SA_OK
;
}
/* Reads the filename in the according property and starts the reading process */
int
SmaractHub
::
PlayRecording
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
return
SA_OK
;
long
isPlay
;
pProp
->
Get
(
isPlay
);
MMThreadGuard
myLock
(
GetLock
());
if
(
isPlay
)
{
if
(
reference_
)
{
SA_STATUS
result
;
for
(
int
i
=
0
;
i
<
6
;
i
++
)
{
result
=
SA_SetReportOnComplete_A
(
mcsHandle_
,
i
,
SA_ENABLED
);
if
(
result
!=
SA_OK
)
return
SA_INITIALIZATION_ERROR
;
}
SA_PACKET
packet
;
do
{
result
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Empties the packet sink
PACKET_TIMEOUT
,
&
packet
);
}
while
(
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
char
*
fileName
=
new
char
[
FILENAME_MAX_SIZE
];
GetProperty
(
"recordingFile"
,
fileName
);
if
(
strcmp
(
fileName
,
" "
)
==
0
)
{
MessageBox
(
nullptr
,
TEXT
(
"ERROR : File not recognised."
),
TEXT
(
"Error"
),
MB_OK
|
MB_ICONERROR
|
MB_SETFOREGROUND
);
return
SA_OK
;
}
OpenExistingFile
(
fileName
);
}
else
{
MessageBox
(
nullptr
,
TEXT
(
"ERROR : Stages are not referenced. Please reference the stages."
),
TEXT
(
"Error"
),
MB_OK
|
MB_ICONERROR
|
MB_SETFOREGROUND
);
}
}
return
SA_OK
;
}
int
SmaractHub
::
OpenExistingFile
(
char
*
fileNameChar
)
{
long
verbose
;
std
::
string
readLine
;
std
::
string
fileName
(
fileNameChar
);
readingFile
.
open
(
"Recordings/"
+
fileName
);
GetProperty
(
"PlayFileVerbose"
,
verbose
);
GetProperty
(
"speedFactor"
,
speedFactor
);
GetProperty
(
"TimeoutOffset"
,
timeoutOffset
);
if
(
speedFactor
<
0.1
)
speedFactor
=
0.1
;
if
(
!
readingFile
.
is_open
())
return
ERR_NO_FILE_FOUND
;
double
pos
;
SA_STATUS
st
=
xTRStage
->
GetPositionUm
(
pos
);
if
(
st
!=
SA_OK
)
return
st
;
lastPositions
[
0
]
=
pos
;
st
=
yTRStage
->
GetPositionUm
(
pos
);
if
(
st
!=
SA_OK
)
return
st
;
lastPositions
[
1
]
=
pos
;
st
=
zTRStage
->
GetPositionUm
(
pos
);
if
(
st
!=
SA_OK
)
return
st
;
lastPositions
[
2
]
=
pos
;
st
=
xROTStage
->
GetPositionUm
(
pos
);
if
(
st
!=
SA_OK
)
return
st
;
lastPositions
[
3
]
=
pos
;
st
=
yROTStage
->
GetPositionUm
(
pos
);
if
(
st
!=
SA_OK
)
return
st
;
lastPositions
[
4
]
=
pos
;
SA_STATUS
result
;
SA_PACKET
packet
;
long
offset
;
GetProperty
(
"XOffset"
,
offset
);
ReplayOffset
[
0
]
=
offset
;
GetProperty
(
"YOffset"
,
offset
);
ReplayOffset
[
1
]
=
offset
;
GetProperty
(
"ZOffset"
,
offset
);
ReplayOffset
[
2
]
=
offset
;
while
(
!
readingFile
.
eof
())
{
std
::
getline
(
readingFile
,
readLine
);
st
=
DecodeAndAct
(
readLine
);
if
(
st
!=
SA_OK
&&
st
!=
ERR_OUT_OF_BOUNDS
)
return
st
;
else
if
(
st
==
ERR_OUT_OF_BOUNDS
)
break
;
}
do
{
result
=
SA_ReceiveNextPacket_A
(
mcsHandle_
,
// Empties the packet sink
PACKET_TIMEOUT
,
// to remove the SA_COMPLETED packets
&
packet
);
}
while
(
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
readingFile
.
close
();
CDeviceUtils
::
SleepMs
(
500
);
if
(
verbose
)
MessageBox
(
nullptr
,
TEXT
(
"Recorded file movement completed"
),
TEXT
(
"Message"
),
MB_OK
|
MB_ICONINFORMATION
|
MB_SETFOREGROUND
);
SetProperty
(
"PlayRecording"
,
"0"
);
return
SA_OK
;
}
int
SmaractHub
::
DecodeAndAct
(
std
::
string
const
&
line
)
{
long
long
timeout
=
0
;
long
long
ctimeout
=
0
;
long
long
maxTimeout
=
MAX_REPLAY_TIMEOUT
/
speedFactor
;
const
char
*
stages
[
7
]
=
{
"X"
,
"Y"
,
"Z"
,
"Alpha"
,
"Beta"
,
"ToolActuator"
,
"Speedlayer"
};
int
i
;
int
index
;
int
strIndex1
;
int
strIndex2
;
int
counter
=
0
;
double
speedLayer
;
long
long
speed
[
6
];
long
long
distance
[
6
];
double
position
[
6
];
std
::
string
stageString
;
std
::
string
stringPos
;
std
::
string
stringSpeed
;
std
::
stringstream
stringStream
;
SA_STATUS
st
;
SA_PACKET
packet
;
for
(
i
=
0
;
i
<=
5
;
i
++
)
{
strIndex1
=
line
.
find
(
stages
[
i
]);
strIndex2
=
line
.
find
(
stages
[
i
+
1
]);
stringStream
.
str
(
line
.
substr
(
strIndex1
,
(
strIndex2
-
strIndex1
)));
stringStream
>>
stageString
;
stringStream
>>
stringPos
;
position
[
i
]
=
std
::
stod
(
stringPos
);
}
position
[
0
]
+=
(
ReplayOffset
[
0
]
*
1000
);
position
[
1
]
+=
(
ReplayOffset
[
1
]
*
1000
);
position
[
2
]
+=
(
ReplayOffset
[
2
]
*
1000
);
if
(
position
[
0
]
<
minTravelLinear
||
position
[
0
]
>
maxTravelLinear
||
position
[
1
]
<
minTravelLinear
||
position
[
1
]
>
maxTravelLinear
||
position
[
2
]
<
minTravelLinear
||
position
[
2
]
>
maxTravelLinear
)
{
MessageBox
(
nullptr
,
TEXT
(
"ERROR : the stage tries to move out of bounds. Stopping the replaying."
),
TEXT
(
"Error"
),
MB_OK
|
MB_ICONERROR
|
MB_SETFOREGROUND
);
return
ERR_OUT_OF_BOUNDS
;
}
strIndex1
=
line
.
find
(
stages
[
6
]);
stringStream
.
str
(
line
.
substr
(
strIndex1
));
stringStream
>>
stringSpeed
;
stringStream
>>
stringSpeed
;
speedLayer
=
std
::
stod
(
stringSpeed
);
speedLayer
*=
speedFactor
;
stringStream
.
str
(
std
::
string
());
stringStream
.
clear
();
for
(
i
=
0
;
i
<=
4
;
i
++
)
{
distance
[
i
]
=
abs
(
position
[
i
]
-
lastPositions
[
i
]);
lastPositions
[
i
]
=
position
[
i
];
speed
[
i
]
=
(
int
)((
double
)
clvMax
*
speedLayer
/
2000
);
ctimeout
=
(
static_cast
<
long
long
>
(
distance
[
i
])
*
1000
)
/
static_cast
<
long
long
>
(
speed
[
i
]);
if
(
ctimeout
>
timeout
)
{
timeout
=
ctimeout
;
index
=
i
;
}
}
if
(
timeout
>
maxTimeout
)
{
speed
[
index
]
=
1000
*
static_cast
<
long
long
>
(
distance
[
index
])
/
maxTimeout
;
timeout
=
maxTimeout
;
if
(
speed
[
index
]
>
MAX_REPLAY_SPEED
)
{
speed
[
index
]
=
MAX_REPLAY_SPEED
;
timeout
=
(
static_cast
<
long
long
>
(
distance
[
index
])
*
1000
)
/
static_cast
<
long
long
>
(
speed
[
index
]);
//ms
}
}
if
(
timeout
<=
0
)
timeout
=
1
;
for
(
i
=
0
;
i
<=
4
;
i
++
)
{
speed
[
i
]
=
1000
*
static_cast
<
long
long
>
(
distance
[
i
])
/
timeout
;
st
=
SA_SetClosedLoopMoveSpeed_A
(
getMCSHandle
(),
i
,
speed
[
i
]);
if
(
st
!=
SA_OK
)
return
st
;
}
timeout
-=
timeoutOffset
;
//shifting timeout to smoothen movement
if
(
timeout
<=
0
)
timeout
=
1
;
do
{
SA_ReceiveNextPacket_A
(
getMCSHandle
(),
// This empties the packet sink
PACKET_TIMEOUT
,
&
packet
);
}
while
(
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
for
(
i
=
0
;
i
<=
2
;
i
++
)
{
SA_GotoPositionAbsolute_A
(
getMCSHandle
(),
i
,
static_cast
<
signed
int
>
(
position
[
i
]),
holdTime
);
}
for
(
i
=
3
;
i
<=
4
;
i
++
)
{
SA_GotoAngleAbsolute_A
(
getMCSHandle
(),
i
,
static_cast
<
signed
int
>
(
position
[
i
]),
0
,
holdTime
);
}
toolActuator
->
SetPositionSteps
(
static_cast
<
signed
int
>
(
position
[
5
]));
CDeviceUtils
::
SleepMs
(
timeout
);
return
SA_OK
;
/*
do
{
SA_ReceiveNextPacket_A(getMCSHandle(), // This empties the packet sink
PACKET_TIMEOUT,
&packet);
if(packet.packetType == SA_COMPLETED_PACKET_TYPE || packet.packetType == SA_NO_PACKET_TYPE)
{
counter ++;
}
}while (counter <= 5);
*/
/*
stringStream.str(std::string());
stringStream.clear();
*/
/*
long long timeout = 0;
while(toolActuator->Busy() == true && timeout < PACKET_TIMEOUT) // Waits until the tool actuator has done the last movement.
{
CDeviceUtils::SleepMs(50);
timeout++;
}
double position;
double speedLayer;
long long speed;
std::string stageString;
std::string stringPos;
std::string stringSpeed;
std::istringstream stringStream(line);
stringStream >> stageString;
stringStream >> stringPos;
if(strcmp(stageString.c_str(), g_DeviceNameToolActuator) == 0)
{
position = std::stod(stringPos);
toolActuator->SetPositionSteps(static_cast<signed int>(position));
return;
}
stringStream >> stringSpeed;
position = std::stod(stringPos); //Value is in nanometers or uー
speedLayer = std::stod(stringSpeed);
SA_INDEX tempChannel = getChannel(stageString.c_str());
//clvMax * abs(vector)*speedProp/2000 FROM TR STAGE
//clvMax * abs(vector)*speedProp/2000 FROM ROT STAGE
speed = (int)((double)clvMax*speedLayer/2000);
long distance = abs(lastPositions[tempChannel] - position);
lastPositions[tempChannel] = position;
timeout = (static_cast<long long>(distance)*1000)/static_cast<long long>(speed); //ms
if(timeout > MAX_REPLAY_TIMEOUT)
{
speed = 1000*static_cast<long long>(distance)/MAX_REPLAY_TIMEOUT;
timeout = MAX_REPLAY_TIMEOUT;
if(speed > MAX_REPLAY_SPEED)
{
speed = MAX_REPLAY_SPEED;
timeout = (static_cast<long long>(distance)*1000)/static_cast<long long>(speed); //ms
}
}
timeout += 100; //adding a 100 ms margin
SA_STATUS st = SA_SetClosedLoopMoveSpeed_A(getMCSHandle(), tempChannel, speed);
if (st != SA_OK)
return;
SA_PACKET packet;
do
{
SA_ReceiveNextPacket_A(getMCSHandle(), // This empties the packet sink
PACKET_TIMEOUT,
&packet);
}while (packet.packetType != SA_NO_PACKET_TYPE);
if(tempChannel != XROT_CHANNEL && tempChannel != YROT_CHANNEL)
SA_GotoPositionAbsolute_A(getMCSHandle(), tempChannel, static_cast<signed int>(position), holdTime);
else
SA_GotoAngleAbsolute_A(getMCSHandle(), tempChannel, static_cast<signed int>(position), 0, holdTime);
do
{
SA_ReceiveNextPacket_A(getMCSHandle(), // This empties the packet sink
timeout,
&packet);
}while (packet.packetType != SA_COMPLETED_PACKET_TYPE && packet.packetType != SA_NO_PACKET_TYPE);
*/
}
/*
* Translational stages
*/
SmaractTrStage
::
SmaractTrStage
(
const
char
*
devName
)
:
initialized_
(
false
),
name_
(
devName
),
channel_
(
255
),
hasSensor_
(
true
),
olMoveScale_
(
1.0
),
minPos_
(
minTravelLinear
),
maxPos_
(
maxTravelLinear
),
olAmplitude_
(
olAmplitude
/
2
),
olFrequency_
(
olFrequency
/
2
),
olMaxSteps_
(
olMaxSteps
/
10
),
async_
(
false
)
{
InitializeDefaultErrorMessages
();
for
(
SA_STATUS
i
=
0
;
i
<
256
;
i
++
)
{
const
char
*
info
;
SA_GetStatusInfo
(
i
,
&
info
);
SetErrorText
(
i
,
&
info
[
0
]);
}
SetErrorText
(
ERR_UNEXPECTED_PACKET
,
g_Msg_ERR_UNEXPECTED_PACKET
);
int
ret
=
CreateProperty
(
MM
::
g_Keyword_Name
,
devName
,
MM
::
String
,
true
);
assert
(
DEVICE_OK
==
ret
);
ret
=
CreateProperty
(
MM
::
g_Keyword_Description
,
"Smaract Translation Stage"
,
MM
::
String
,
true
);
assert
(
DEVICE_OK
==
ret
);
CreateProperty
(
"Direction"
,
"1"
,
MM
::
Integer
,
false
,
0
,
true
);
AddAllowedValue
(
"Direction"
,
"-1"
);
AddAllowedValue
(
"Direction"
,
"1"
);
CreateHubIDProperty
();
}
SmaractTrStage
::~
SmaractTrStage
()
{
Shutdown
();
}
/*int SmaractTrStage::GetPositionSteps(long &steps)
{
SmaractHub* hub = static_cast<SmaractHub*>(GetParentHub());
if (!hub || !hub->IsConnected())
{
return ERR_HUB_NOT_CONNECTED;
}
MMThreadGuard myLock(hub->GetLock());
signed int pos;
ExitIfError(SA_GetPosition_S(hub->getMCSHandle(), channel_, &pos));
steps = static_cast<long>(pos);
return DEVICE_OK;
}*/
int
SmaractTrStage
::
Initialize
()
{
if
(
initialized_
==
false
)
{
SA_STATUS
st
;
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
return
ERR_HUB_NOT_CONNECTED
;
char
hubLabel
[
MM
::
MaxStrLength
];
hub
->
GetLabel
(
hubLabel
);
SetParentID
(
hubLabel
);
async_
=
hub
->
isCommModeAsync
();
MMThreadGuard
myLock
(
hub
->
GetLock
());
channel_
=
hub
->
getChannel
(
name_
.
c_str
());
if
(
strcmp
(
name_
.
c_str
(),
g_DeviceNameSmaractGamma
)
==
0
)
hasSensor_
=
false
;
else
hasSensor_
=
true
;
if
(
hasSensor_
)
{
if
(
async_
)
{
st
=
SA_SetReportOnComplete_A
(
hub
->
getMCSHandle
(),
channel_
,
SA_DISABLED
);
st
=
SA_SetSafeDirection_A
(
hub
->
getMCSHandle
(),
channel_
,
SA_FORWARD_DIRECTION
);
if
(
st
!=
SA_OK
)
return
st
;
if
(
hub
->
isCalibrate
())
{
SA_CalibrateSensor_A
(
hub
->
getMCSHandle
(),
channel_
);
SA_STATUS
calibrationStatus
;
do
{
CDeviceUtils
::
SleepMs
(
1000
);
st
=
SA_GetStatus_A
(
hub
->
getMCSHandle
(),
channel_
);
SA_PACKET
packet
;
st
=
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
1000
,
&
packet
);
if
(
st
!=
SA_OK
)
return
st
;
else
{
if
((
packet
.
packetType
==
SA_STATUS_PACKET_TYPE
)
&&
(
packet
.
channelIndex
==
channel_
))
calibrationStatus
=
packet
.
data1
;
else
if
(
packet
.
packetType
==
SA_ERROR_PACKET_TYPE
)
if
(
packet
.
data1
!=
SA_END_STOP_REACHED_ERROR
)
// this is normal
return
packet
.
data1
;
else
if
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
}
}
while
(
calibrationStatus
==
SA_CALIBRATING_STATUS
);
}
st
=
SA_SetClosedLoopMoveSpeed_A
(
hub
->
getMCSHandle
(),
channel_
,
clvMax
/
10
*
9
);
if
(
st
!=
SA_OK
)
return
st
;
/*
if (hub->isReference())
{
SA_FindReferenceMark_A(hub->getMCSHandle(), channel_, SA_FORWARD_DIRECTION, 0, SA_AUTO_ZERO);
SA_STATUS findrefStatus;
do {
CDeviceUtils::SleepMs(1000);
st = SA_GetStatus_A(hub->getMCSHandle(), channel_);
SA_PACKET packet;
st = SA_ReceiveNextPacket_A(hub->getMCSHandle(), 1000, &packet);
if (st != SA_OK)
return st;
else
{
if (packet.channelIndex == channel_)
{
if (packet.packetType == SA_STATUS_PACKET_TYPE)
findrefStatus = packet.data1;
else if (packet.packetType == SA_ERROR_PACKET_TYPE)
if (packet.data1 == SA_COULD_NOT_FIND_REF_ERROR)
break; // Discard this error, there is no reference mark, although there is a sensor.
else
return packet.data1;
else
return ERR_UNEXPECTED_PACKET;
}
}
} while (findrefStatus == SA_FINDING_REF_STATUS);
CDeviceUtils::SleepMs(1000); // ms
st = SA_SetPosition_A(hub->getMCSHandle(), channel_, 0);
if (st != SA_OK)
return st;
signed int midWay = (maxPos_ - minPos_) / 2;
st = SA_GotoPositionAbsolute_A(hub->getMCSHandle(), channel_, midWay, holdTime);
if (st != SA_OK)
return st;
st = SA_SetPosition_A(hub->getMCSHandle(), channel_, 0);
if (st != SA_OK)
return st;
SA_STATUS moveStatus;
do {
CDeviceUtils::SleepMs(1000);
st = SA_GetStatus_A(hub->getMCSHandle(), channel_);
SA_PACKET packet;
st = SA_ReceiveNextPacket_A(hub->getMCSHandle(), 1000, &packet);
if (st != SA_OK)
return st;
else
{
if (packet.channelIndex == channel_)
{
if (packet.packetType == SA_STATUS_PACKET_TYPE)
moveStatus = packet.data1;
else if (packet.packetType == SA_ERROR_PACKET_TYPE)
if (packet.data1 != SA_END_STOP_REACHED_ERROR) // this is normal
return packet.data1;
else if (packet.packetType != SA_COMPLETED_PACKET_TYPE)
return ERR_UNEXPECTED_PACKET;
}
}
} while (moveStatus == SA_TARGET_STATUS);
SA_PACKET packet;
st = SA_ReceiveNextPacket_A(hub->getMCSHandle(), 1000, &packet); // Retrieve tha last packet
}
*/
}
else
{
SA_STATUS
st
=
SA_SetSafeDirection_S
(
hub
->
getMCSHandle
(),
channel_
,
SA_FORWARD_DIRECTION
);
if
(
st
!=
SA_OK
)
return
st
;
if
(
hub
->
isCalibrate
())
{
SA_CalibrateSensor_S
(
hub
->
getMCSHandle
(),
channel_
);
do
{
SA_GetStatus_S
(
hub
->
getMCSHandle
(),
channel_
,
&
st
);
}
while
(
st
!=
SA_STOPPED_STATUS
);
}
if
(
hub
->
isReference
())
{
SA_FindReferenceMark_S
(
hub
->
getMCSHandle
(),
channel_
,
SA_FORWARD_DIRECTION
,
0
,
SA_AUTO_ZERO
);
do
{
SA_GetStatus_S
(
hub
->
getMCSHandle
(),
channel_
,
&
st
);
}
while
(
st
!=
SA_STOPPED_STATUS
);
if
(
st
!=
SA_OK
)
return
st
;
}
// Physical position can not be known since we have hard limit switches
unsigned
int
known
;
st
=
SA_GetPhysicalPositionKnown_S
(
hub
->
getMCSHandle
(),
channel_
,
&
known
);
if
(
st
==
SA_OK
)
{
if
(
known
!=
SA_PHYSICAL_POSITION_KNOWN
)
;
}
else
{
if
(
known
!=
SA_PHYSICAL_POSITION_KNOWN
)
;
}
CDeviceUtils
::
SleepMs
(
1000
);
// ms
st
=
SA_SetClosedLoopMoveSpeed_S
(
hub
->
getMCSHandle
(),
channel_
,
clvMax
/
10
*
9
);
if
(
st
!=
SA_OK
)
return
st
;
if
(
hub
->
isReference
())
{
st
=
SA_SetPosition_S
(
hub
->
getMCSHandle
(),
channel_
,
0
);
if
(
st
!=
SA_OK
)
return
st
;
signed
int
midWay
=
(
maxPos_
-
minPos_
)
/
2
;
st
=
SA_GotoPositionAbsolute_S
(
hub
->
getMCSHandle
(),
channel_
,
midWay
,
holdTime
);
if
(
st
!=
SA_OK
)
return
st
;
st
=
SA_SetPosition_S
(
hub
->
getMCSHandle
(),
channel_
,
0
);
if
(
st
!=
SA_OK
)
return
st
;
}
}
minPos_
+=
dSafe_nm
;
maxPos_
-=
dSafe_nm
;
}
// Open Loop move action property
CPropertyAction
*
pAct
=
new
CPropertyAction
(
this
,
&
SmaractTrStage
::
OnOLMove
);
CreateProperty
(
"OLMove"
,
"0"
,
MM
::
Float
,
false
,
pAct
);
SetPropertyLimits
(
"OLMove"
,
-
olMoveScale_
,
olMoveScale_
);
UpdateProperty
(
"OLMove"
);
pAct
=
new
CPropertyAction
(
this
,
&
SmaractTrStage
::
OnOLStep
);
CreateProperty
(
"OLStep"
,
"-1"
,
MM
::
String
,
false
,
pAct
);
UpdateProperty
(
"OLStep"
);
hub
->
SetTRStage
(
name_
.
c_str
(),
this
);
initialized_
=
true
;
}
return
DEVICE_OK
;
}
int
SmaractTrStage
::
Shutdown
()
{
initialized_
=
false
;
return
DEVICE_OK
;
}
void
SmaractTrStage
::
GetName
(
char
*
name
)
const
{
CDeviceUtils
::
CopyLimitedString
(
name
,
name_
.
c_str
());
}
std
::
string
SmaractTrStage
::
ExportName
()
{
return
name_
;
}
bool
SmaractTrStage
::
Busy
()
{
if
(
async_
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
return
ERR_HUB_NOT_CONNECTED
;
SA_STATUS
st
=
SA_GetStatus_A
(
hub
->
getMCSHandle
(),
channel_
);
SA_PACKET
packet
;
st
=
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
1000
,
&
packet
);
if
(
st
!=
SA_OK
)
return
st
;
if
((
packet
.
packetType
==
SA_STATUS_PACKET_TYPE
)
&&
(
packet
.
channelIndex
==
0
))
{
if
(
packet
.
data1
==
SA_STOPPED_STATUS
||
packet
.
data1
==
SA_HOLDING_STATUS
)
return
false
;
// Not busy
else
return
true
;
}
}
else
return
false
;
}
int
SmaractTrStage
::
GetLimits
(
double
&
pMin
,
double
&
pMax
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
MMThreadGuard
myLock
(
hub
->
GetLock
());
signed
int
posMin
,
posMax
;
if
(
async_
)
{
SA_STATUS
st
=
SA_GetPositionLimit_A
(
hub
->
getMCSHandle
(),
channel_
);
SA_PACKET
packet
;
st
=
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
)
return
st
;
else
{
if
((
packet
.
packetType
==
SA_POSITION_LIMIT_PACKET_TYPE
)
&&
(
packet
.
channelIndex
==
channel_
))
{
posMin
=
packet
.
data2
;
posMax
=
packet
.
data3
;
}
else
return
ERR_UNEXPECTED_PACKET
;
}
}
else
{
SA_STATUS
st
=
SA_GetPositionLimit_S
(
hub
->
getMCSHandle
(),
channel_
,
&
posMin
,
&
posMax
);
if
(
st
!=
SA_OK
)
return
st
;
}
pMin
=
static_cast
<
double
>
(
posMin
);
pMax
=
static_cast
<
double
>
(
posMax
);
return
DEVICE_OK
;
}
int
SmaractTrStage
::
GetPositionUm
(
double
&
pos
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
signed
int
position
;
MMThreadGuard
myLock
(
hub
->
GetLock
());
if
(
async_
)
{
SA_PACKET
packet
;
do
{
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
// This empties the packet sink
PACKET_TIMEOUT
,
&
packet
);
}
while
(
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
SA_STATUS
st
=
SA_GetPosition_A
(
hub
->
getMCSHandle
(),
channel_
);
st
=
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
)
return
st
;
else
{
if
((
packet
.
packetType
==
SA_POSITION_PACKET_TYPE
)
&&
(
packet
.
channelIndex
==
channel_
))
position
=
packet
.
data2
;
else
return
ERR_UNEXPECTED_PACKET
;
}
}
else
{
SA_STATUS
st
=
SA_GetPosition_S
(
hub
->
getMCSHandle
(),
channel_
,
&
position
);
if
(
st
!=
SA_OK
)
return
st
;
}
// The value is in nanometers
pos
=
static_cast
<
double
>
(
position
);
return
DEVICE_OK
;
}
int
SmaractTrStage
::
SetPositionUm
(
double
pos
)
{
SA_STATUS
st
;
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
// Here the value is converted to mm
signed
int
position
=
static_cast
<
signed
int
>
(
pos
)
*
1000
;
MMThreadGuard
myLock
(
hub
->
GetLock
());
// TODO: Clarify hold time to use
if
(
async_
)
st
=
SA_GotoPositionAbsolute_A
(
hub
->
getMCSHandle
(),
channel_
,
position
,
holdTime
);
else
st
=
SA_GotoPositionAbsolute_S
(
hub
->
getMCSHandle
(),
channel_
,
position
,
holdTime
);
if
(
st
!=
SA_OK
)
return
st
;
return
DEVICE_OK
;
}
int
SmaractTrStage
::
SetRelativePositionUm
(
double
dPos
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
return
ERR_HUB_NOT_CONNECTED
;
// Here the value is converted to mm
signed
int
dPosition
=
static_cast
<
signed
int
>
(
dPos
)
*
1000
;
MMThreadGuard
myLock
(
hub
->
GetLock
());
SA_STATUS
st
;
if
(
async_
)
st
=
SA_GotoPositionRelative_A
(
hub
->
getMCSHandle
(),
channel_
,
dPosition
,
holdTime
);
else
st
=
SA_GotoPositionRelative_S
(
hub
->
getMCSHandle
(),
channel_
,
dPosition
,
holdTime
);
if
(
st
!=
SA_OK
)
return
st
;
return
DEVICE_OK
;
}
/*int SmaractTrStage::SetPositionSteps(long steps)
{
SmaractHub* hub = static_cast<SmaractHub*>(GetParentHub());
if (!hub || !hub->IsConnected())
{
return ERR_HUB_NOT_CONNECTED;
}
MMThreadGuard myLock(hub->GetLock());
ExitIfError(SA_StepMove_S(hub->getMCSHandle(), channel_, steps, 1500, 2000));
return DEVICE_OK;
}*/
int
SmaractTrStage
::
SetOrigin
()
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
MMThreadGuard
myLock
(
hub
->
GetLock
());
SA_STATUS
st
;
if
(
async_
)
st
=
SA_SetPosition_A
(
hub
->
getMCSHandle
(),
channel_
,
0
);
else
st
=
SA_SetPosition_S
(
hub
->
getMCSHandle
(),
channel_
,
0
);
if
(
st
!=
SA_OK
)
return
st
;
return
DEVICE_OK
;
}
int
SmaractTrStage
::
SetFrequency
(
int
f
)
{
unsigned
int
freq
=
static_cast
<
unsigned
int
>
(
f
);
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
return
ERR_HUB_NOT_CONNECTED
;
MMThreadGuard
myLock
(
hub
->
GetLock
());
SA_STATUS
st
;
if
(
async_
)
st
=
SA_SetClosedLoopMaxFrequency_A
(
hub
->
getMCSHandle
(),
channel_
,
freq
);
else
st
=
SA_SetClosedLoopMaxFrequency_S
(
hub
->
getMCSHandle
(),
channel_
,
freq
);
if
(
st
!=
SA_OK
)
return
st
;
return
DEVICE_OK
;
}
int
SmaractTrStage
::
SetErrorReporting
(
boolean
reporting
)
{
return
DEVICE_UNSUPPORTED_COMMAND
;
}
// action interface
int
SmaractTrStage
::
OnLimit
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
return
DEVICE_UNSUPPORTED_COMMAND
;
}
int
SmaractTrStage
::
OnFrequency
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
return
DEVICE_UNSUPPORTED_COMMAND
;
}
int
SmaractTrStage
::
Move
(
double
velocity
)
{
return
DEVICE_UNSUPPORTED_COMMAND
;
}
int
SmaractTrStage
::
OnOLMove
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
return
ERR_HUB_NOT_CONNECTED
;
MMThreadGuard
myLock
(
hub
->
GetLock
());
SA_STATUS
st
;
if
(
eAct
==
MM
::
BeforeGet
)
{
/*double speed;
unsigned int stat;
SA_GetStatus_S(hub->getMCSHandle(), channel_, &stat);
if (stat == SA_STEPPING_STATUS)
{
speed = 0.0;
}
else
{
speed = 0.0;
}
pProp->Set(speed);*/
}
else
if
(
eAct
==
MM
::
AfterSet
)
{
double
vector
;
pProp
->
Get
(
vector
);
if
(
abs
(
vector
)
>
1.0
)
return
ERR_ASIGP_BAD_MULT
;
//Changed the threshold value to avoid noise movement IMPORTANT TO CHECK : Using the right side of the left thumbstick seems to be faster
if
(
abs
(
vector
)
<
0.25
)
{
SA_STATUS
st
;
if
(
async_
)
st
=
SA_Stop_A
(
hub
->
getMCSHandle
(),
channel_
);
else
st
=
SA_Stop_S
(
hub
->
getMCSHandle
(),
channel_
);
if
(
st
!=
SA_OK
)
return
st
;
}
else
{
/*
* Recording data (stage name, vector, speedlayer)
*/
long
recording
;
SA_STATUS
st
=
hub
->
GetProperty
(
"Recording"
,
recording
);
if
(
st
!=
SA_OK
)
return
st
;
long
speedLayer
;
st
=
hub
->
GetProperty
(
"speedLayer"
,
speedLayer
);
if
(
st
!=
SA_OK
)
return
st
;
if
(
recording
==
1
)
{
hub
->
Snapshot
();
/*
double recpos;
GetPositionUm(recpos);
if(recpos > minTravelLinear && recpos < maxTravelLinear)
{
std::string recordData = name_ + " "
+ std::to_string(static_cast<long long>(recpos)) + " "
+ std::to_string(static_cast<long long>(speedLayer));
hub->WriteToRecordingFile(recordData);
}
*/
}
if
(
async_
)
{
long
speedProp
;
SA_STATUS
st
=
hub
->
GetProperty
(
"speedLayer"
,
speedProp
);
if
(
st
!=
SA_OK
)
return
st
;
// Changed the speed value by dividing the speed variable
unsigned
int
speed
=
static_cast
<
unsigned
int
>
((
double
)
clvMax
*
abs
(
vector
)
*
hub
->
speedLayersValue
[
speedProp
]
/
2000
)
;
/*
char buffer[100];
sprintf(buffer, "speed %d, vector %lf, speedProp %d\n",speed, vector, speedProp);
OutputDebugStringA(buffer);
*/
int
pos
;
if
(
vector
<
0.0
)
pos
=
minPos_
;
else
pos
=
maxPos_
;
st
=
SA_SetClosedLoopMoveSpeed_A
(
hub
->
getMCSHandle
(),
channel_
,
speed
);
if
(
st
!=
SA_OK
)
return
st
;
SA_GotoPositionAbsolute_A
(
hub
->
getMCSHandle
(),
channel_
,
pos
,
holdTime
);
}
else
{
int
stepsToMove
;
unsigned
int
amp_int
,
freq_int
;
double
amp_d
,
freq_d
,
steps_d
;
amp_d
=
static_cast
<
double
>
(
olAmplitude_
)
*
abs
(
vector
);
freq_d
=
static_cast
<
double
>
(
olFrequency_
)
*
abs
(
vector
);
steps_d
=
static_cast
<
double
>
(
olMaxSteps_
)
*
vector
;
amp_int
=
static_cast
<
unsigned
int
>
(
amp_d
);
freq_int
=
static_cast
<
unsigned
int
>
(
freq_d
);
stepsToMove
=
static_cast
<
int
>
(
steps_d
);
SA_STATUS
st
=
SA_StepMove_S
(
hub
->
getMCSHandle
(),
channel_
,
stepsToMove
,
amp_int
,
freq_int
);
if
(
st
!=
SA_OK
)
return
st
;
}
}
}
return
DEVICE_OK
;
}
int
SmaractTrStage
::
OnOLStep
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
return
ERR_HUB_NOT_CONNECTED
;
MMThreadGuard
myLock
(
hub
->
GetLock
());
if
(
eAct
==
MM
::
BeforeGet
)
{
}
else
if
(
eAct
==
MM
::
AfterSet
)
{
std
::
string
whichDPad
;
pProp
->
Get
(
whichDPad
);
//int whichDPad_int = static_cast<int>(whichDPad);
SA_STATUS
st
;
if
(
strcmp
(
whichDPad
.
c_str
(),
"1"
)
==
0
)
// Up
{
st
=
SA_StepMove_S
(
hub
->
getMCSHandle
(),
channel_
,
olMaxSteps_
,
olAmplitude_
,
olFrequency_
);
if
(
st
!=
SA_OK
)
return
st
;
}
else
if
(
strcmp
(
whichDPad
.
c_str
(),
"3"
)
==
0
)
// Right
{
}
else
if
(
strcmp
(
whichDPad
.
c_str
(),
"5"
)
==
0
)
// Down
{
st
=
SA_StepMove_S
(
hub
->
getMCSHandle
(),
channel_
,
-
olMaxSteps_
,
olAmplitude_
,
olFrequency_
);
if
(
st
!=
SA_OK
)
return
st
;
}
else
if
(
strcmp
(
whichDPad
.
c_str
(),
"7"
)
==
0
)
// Left
{
}
else
// Not pressed (= -1)
{
}
}
return
DEVICE_OK
;
}
void
SmaractTrStage
::
testMoveCallback
()
{
}
/*
* Rotational stages
*/
SmaractRotStage
::
SmaractRotStage
(
const
char
*
devName
)
:
initialized_
(
false
),
name_
(
devName
),
channel_
(
255
),
hasSensor_
(
true
),
olMoveScale_
(
1.0
),
minAng_
(
0
),
maxAng_
(
0
),
posSteps_
(
0
),
olAmplitude_
(
olAmplitude
/
2
),
olFrequency_
(
olFrequency
/
2
),
olMaxSteps_
(
olMaxSteps
),
async_
(
false
)
{
if
(
strcmp
(
devName
,
g_DeviceNameSmaractGamma
)
==
0
)
hasSensor_
=
false
;
InitializeDefaultErrorMessages
();
for
(
SA_STATUS
i
=
0
;
i
<
256
;
i
++
)
{
const
char
*
info
;
SA_GetStatusInfo
(
i
,
&
info
);
SetErrorText
(
i
,
&
info
[
0
]);
}
SetErrorText
(
ERR_UNEXPECTED_PACKET
,
g_Msg_ERR_UNEXPECTED_PACKET
);
int
ret
=
CreateProperty
(
MM
::
g_Keyword_Name
,
devName
,
MM
::
String
,
true
);
assert
(
DEVICE_OK
==
ret
);
ret
=
CreateProperty
(
MM
::
g_Keyword_Description
,
"Smaract Rotation Stage"
,
MM
::
String
,
true
);
assert
(
DEVICE_OK
==
ret
);
CreateProperty
(
"Direction"
,
"1"
,
MM
::
Integer
,
false
,
0
,
true
);
AddAllowedValue
(
"Direction"
,
"-1"
);
AddAllowedValue
(
"Direction"
,
"1"
);
CreateHubIDProperty
();
/*
To avoid zero crossing, the angle has been shifted and the systems
scale has also been moved to keep the same movement range.
*/
if
(
strcmp
(
name_
.
c_str
(),
g_DeviceNameSmaractAlpha
)
==
0
)
{
minAng_
=
5000000
;
maxAng_
=
95000000
;
}
else
if
(
strcmp
(
name_
.
c_str
(),
g_DeviceNameSmaractBeta
)
==
0
)
{
minAng_
=
5000000
;
maxAng_
=
95000000
;
}
}
SmaractRotStage
::~
SmaractRotStage
()
{
Shutdown
();
}
int
SmaractRotStage
::
Initialize
()
{
if
(
initialized_
==
false
)
{
SA_STATUS
st
;
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
char
hubLabel
[
MM
::
MaxStrLength
];
hub
->
GetLabel
(
hubLabel
);
SetParentID
(
hubLabel
);
async_
=
hub
->
isCommModeAsync
();
MMThreadGuard
myLock
(
hub
->
GetLock
());
channel_
=
hub
->
getChannel
(
name_
.
c_str
());
if
(
strcmp
(
name_
.
c_str
(),
g_DeviceNameSmaractGamma
)
==
0
)
hasSensor_
=
false
;
else
hasSensor_
=
true
;
if
(
hasSensor_
)
{
if
(
async_
)
{
if
(
hub
->
isCalibrate
())
{
SA_CalibrateSensor_A
(
hub
->
getMCSHandle
(),
channel_
);
SA_STATUS
calibrationStatus
;
do
{
CDeviceUtils
::
SleepMs
(
1000
);
st
=
SA_GetStatus_A
(
hub
->
getMCSHandle
(),
channel_
);
SA_PACKET
packet
;
st
=
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
)
return
st
;
else
{
if
((
packet
.
packetType
==
SA_STATUS_PACKET_TYPE
)
&&
(
packet
.
channelIndex
==
channel_
))
calibrationStatus
=
packet
.
data1
;
else
if
(
packet
.
packetType
==
SA_ERROR_PACKET_TYPE
)
if
(
packet
.
data1
!=
SA_END_STOP_REACHED_ERROR
)
// this is normal
return
packet
.
data1
;
else
if
(
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
)
return
ERR_UNEXPECTED_PACKET
;
}
}
while
(
calibrationStatus
==
SA_CALIBRATING_STATUS
);
}
/*
if (hub->isReference())
{
SA_STATUS movement_st;
st = SA_SetReportOnComplete_A(hub->getMCSHandle(),channel_, SA_DISABLED);
st = SA_FindReferenceMark_A(hub->getMCSHandle(), channel_, SA_FORWARD_DIRECTION, 0, SA_AUTO_ZERO);
if (st != SA_OK)
return st;
SA_STATUS findrefStatus;
do {
CDeviceUtils::SleepMs(1000);
st = SA_GetStatus_A(hub->getMCSHandle(), channel_);
SA_PACKET packet;
st = SA_ReceiveNextPacket_A(hub->getMCSHandle(), PACKET_TIMEOUT, &packet);
if (st != SA_OK)
return st;
else
{
if (packet.channelIndex == channel_)
{
if (packet.packetType == SA_STATUS_PACKET_TYPE)
findrefStatus = packet.data1;
else if ((packet.packetType == SA_COMPLETED_PACKET_TYPE) &&
(packet.channelIndex == channel_))
break;
else
return ERR_UNEXPECTED_PACKET;
}
}
} while (findrefStatus == SA_FINDING_REF_STATUS);
CDeviceUtils::SleepMs(1000); // ms
}
*/
// Check if there is one more status packet
SA_PACKET
packet
;
st
=
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
PACKET_TIMEOUT
,
&
packet
);
if
(
st
!=
SA_OK
)
return
st
;
else
{
bool
test1
=
packet
.
packetType
!=
SA_NO_PACKET_TYPE
;
bool
test2
=
packet
.
packetType
!=
SA_COMPLETED_PACKET_TYPE
;
if
(
test1
==
true
&&
test2
==
true
)
// DEBUG must make this consistent
{
if
((
packet
.
packetType
!=
SA_STATUS_PACKET_TYPE
)
||
(
packet
.
channelIndex
!=
channel_
)
||
packet
.
data1
!=
SA_STOPPED_STATUS
)
return
ERR_UNEXPECTED_PACKET
;
}
}
st
=
SA_SetClosedLoopMoveSpeed_A
(
hub
->
getMCSHandle
(),
channel_
,
clvMax
/
10
*
9
);
if
(
st
!=
SA_OK
)
return
st
;
}
else
{
if
(
hub
->
isCalibrate
())
{
SA_CalibrateSensor_S
(
hub
->
getMCSHandle
(),
channel_
);
do
{
SA_GetStatus_S
(
hub
->
getMCSHandle
(),
channel_
,
&
st
);
}
while
(
st
!=
SA_STOPPED_STATUS
);
}
st
=
SA_SetClosedLoopMoveSpeed_S
(
hub
->
getMCSHandle
(),
channel_
,
clvMax
/
10
*
9
);
if
(
st
!=
SA_OK
)
return
st
;
unsigned
int
minAng
=
360000000
+
minAng_
;
int
minRev
=
-
1
;
if
(
hub
->
isReference
())
{
SA_STATUS
st
=
SA_FindReferenceMark_S
(
hub
->
getMCSHandle
(),
channel_
,
SA_FORWARD_DIRECTION
,
5000
,
SA_AUTO_ZERO
);
do
{
SA_GetStatus_S
(
hub
->
getMCSHandle
(),
channel_
,
&
st
);
}
while
(
st
!=
SA_STOPPED_STATUS
);
if
(
st
!=
SA_OK
)
return
st
;
CDeviceUtils
::
SleepMs
(
1000
);
// ms
st
=
SA_GotoAngleAbsolute_S
(
hub
->
getMCSHandle
(),
channel_
,
minAng
,
minRev
,
holdTime
);
do
{
SA_GetStatus_S
(
hub
->
getMCSHandle
(),
channel_
,
&
st
);
}
while
(
st
!=
SA_STOPPED_STATUS
);
if
(
st
!=
SA_OK
)
return
st
;
st
=
SA_GotoAngleAbsolute_S
(
hub
->
getMCSHandle
(),
channel_
,
maxAng_
,
0
,
holdTime
);
do
{
SA_GetStatus_S
(
hub
->
getMCSHandle
(),
channel_
,
&
st
);
}
while
(
st
!=
SA_STOPPED_STATUS
);
if
(
st
!=
SA_OK
)
return
st
;
st
=
SA_GotoAngleAbsolute_S
(
hub
->
getMCSHandle
(),
channel_
,
0
,
0
,
holdTime
);
do
{
SA_GetStatus_S
(
hub
->
getMCSHandle
(),
channel_
,
&
st
);
}
while
(
st
!=
SA_STOPPED_STATUS
);
if
(
st
!=
SA_OK
)
return
st
;
}
st
=
SA_SetAngleLimit_S
(
hub
->
getMCSHandle
(),
channel_
,
minAng
,
minRev
,
maxAng_
,
0
);
if
(
st
!=
SA_OK
)
return
st
;
}
}
// TODO: set maximal and minimal allowed values here, set home position etc.
// Vector move action property
CPropertyAction
*
pAct
=
new
CPropertyAction
(
this
,
&
SmaractRotStage
::
OnOLMove
);
CreateProperty
(
"OLMove"
,
"0"
,
MM
::
Float
,
false
,
pAct
);
SetPropertyLimits
(
"OLMove"
,
-
olMoveScale_
,
olMoveScale_
);
UpdateProperty
(
"OLMove"
);
hub
->
SetRotStage
(
name_
.
c_str
(),
this
);
initialized_
=
true
;
}
return
DEVICE_OK
;
}
int
SmaractRotStage
::
Shutdown
()
{
/*
Beta rotary stage must be driven into the positive range to avoid referencing in wrong direction
if and only if the stages are in free mode i.e. the speed layer is not the maximum speed
*/
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
MMThreadGuard
myLock
(
hub
->
GetLock
());
long
speedLayer
;
SA_STATUS
st
=
hub
->
GetProperty
(
"speedLayer"
,
speedLayer
);
if
(
st
!=
SA_OK
)
return
st
;
/*
if (name_ == g_DeviceNameSmaractBeta && speedLayer > 100 && hub->isReference())
{
if (async_)
{
SmaractHub* hub = static_cast<SmaractHub*>(GetParentHub());
if (!hub || !hub->IsConnected())
return ERR_HUB_NOT_CONNECTED;
unsigned int speed = static_cast<unsigned int>((double)clvMax * 0.1);
SA_STATUS st = SA_SetClosedLoopMoveSpeed_A(hub->getMCSHandle(), channel_, speed);
if (st != SA_OK)
return st;
st = SA_GotoAngleAbsolute_A(hub->getMCSHandle(), channel_, 40*1000000, 0, holdTime);
if (st != SA_OK)
return st;
}
else
{
SmaractHub* hub = static_cast<SmaractHub*>(GetParentHub());
if (!hub || !hub->IsConnected())
return ERR_HUB_NOT_CONNECTED;
unsigned int speed = static_cast<unsigned int>((double)clvMax * 0.1);
SA_STATUS st = SA_SetClosedLoopMoveSpeed_S(hub->getMCSHandle(), channel_, speed);
if (st != SA_OK)
return st;
st = SA_GotoAngleAbsolute_S(hub->getMCSHandle(), channel_, 40*1000000, 0, holdTime);
if (st != SA_OK)
return st;
}
}
*/
initialized_
=
false
;
return
DEVICE_OK
;
}
void
SmaractRotStage
::
GetName
(
char
*
name
)
const
{
CDeviceUtils
::
CopyLimitedString
(
name
,
name_
.
c_str
());
}
std
::
string
SmaractRotStage
::
ExportName
()
{
return
name_
;
}
bool
SmaractRotStage
::
Busy
()
{
SA_STATUS
st
;
if
(
async_
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
return
ERR_HUB_NOT_CONNECTED
;
st
=
SA_GetStatus_A
(
hub
->
getMCSHandle
(),
channel_
);
SA_PACKET
packet
;
st
=
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
1000
,
&
packet
);
if
(
st
!=
SA_OK
)
return
st
;
if
((
packet
.
packetType
==
SA_STATUS_PACKET_TYPE
)
&&
(
packet
.
channelIndex
==
0
))
{
if
(
packet
.
data1
==
SA_STOPPED_STATUS
||
packet
.
data1
==
SA_HOLDING_STATUS
)
return
false
;
// Not busy
else
return
true
;
}
}
else
return
false
;
}
// Returns an angle in udegree rather than steps
int
SmaractRotStage
::
GetPositionUm
(
double
&
angT
)
{
SA_STATUS
st
;
if
(
hasSensor_
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
MMThreadGuard
myLock
(
hub
->
GetLock
());
signed
int
revolution
;
unsigned
int
angle
;
if
(
async_
)
{
SA_PACKET
packet
;
do
{
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
// This empties the packet sink
10
,
&
packet
);
}
while
(
packet
.
packetType
!=
SA_NO_PACKET_TYPE
);
st
=
SA_GetAngle_A
(
hub
->
getMCSHandle
(),
channel_
);
st
=
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
1000
,
&
packet
);
if
(
st
!=
SA_OK
)
return
st
;
else
{
if
((
packet
.
packetType
==
SA_ANGLE_PACKET_TYPE
)
&&
(
packet
.
channelIndex
==
channel_
))
{
angle
=
packet
.
data1
;
revolution
=
packet
.
data2
;
}
else
return
ERR_UNEXPECTED_PACKET
;
}
}
else
{
st
=
SA_GetAngle_S
(
hub
->
getMCSHandle
(),
channel_
,
&
angle
,
&
revolution
);
if
(
st
!=
SA_OK
)
return
st
;
}
// Angle is in uー
angT
=
(
static_cast
<
double
>
(
angle
)
+
360000000
*
static_cast
<
double
>
(
revolution
));
}
else
{
angT
=
static_cast
<
double
>
(
posSteps_
);
}
return
DEVICE_OK
;
}
// Returns [uー]
int
SmaractRotStage
::
GetLimits
(
double
&
minAngT
,
double
&
maxAngT
)
{
SA_STATUS
st
;
unsigned
int
minAng
,
maxAng
;
signed
int
minRev
,
maxRev
;
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
MMThreadGuard
myLock
(
hub
->
GetLock
());
if
(
async_
)
{
st
=
SA_GetAngleLimit_A
(
hub
->
getMCSHandle
(),
channel_
);
SA_PACKET
packet
;
st
=
SA_ReceiveNextPacket_A
(
hub
->
getMCSHandle
(),
1000
,
&
packet
);
if
(
st
!=
SA_OK
)
return
st
;
else
{
if
((
packet
.
packetType
==
SA_POSITION_LIMIT_PACKET_TYPE
)
&&
(
packet
.
channelIndex
==
channel_
))
{
minAng
=
packet
.
data1
;
minRev
=
packet
.
data2
;
maxAng
=
packet
.
data4
;
maxRev
=
packet
.
data3
;
}
else
return
ERR_UNEXPECTED_PACKET
;
}
}
else
{
st
=
SA_GetAngleLimit_S
(
hub
->
getMCSHandle
(),
channel_
,
&
minAng
,
&
minRev
,
&
maxAng
,
&
maxRev
);
if
(
st
!=
SA_OK
)
return
st
;
}
minAngT
=
static_cast
<
double
>
(
minAng
)
+
2.0
*
M_PI
*
static_cast
<
double
>
(
minRev
)
*
1000.0
;
maxAngT
=
static_cast
<
double
>
(
maxAng
)
+
2.0
*
M_PI
*
static_cast
<
double
>
(
maxRev
)
*
1000.0
;
return
DEVICE_OK
;
}
// This function actually takes [uー] instead of [um]
// TODO: Bug here with negative values and calculation
int
SmaractRotStage
::
SetRelativePositionUm
(
double
dAngT
)
{
SA_STATUS
st
;
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
if
(
hasSensor_
)
{
int
dAngT_int
=
static_cast
<
signed
int
>
(
dAngT
);
int
dAngle
,
dRev
;
dAngle
=
dAngT_int
%
360000000
;
dRev
=
dAngT_int
/
360000000
;
MMThreadGuard
myLock
(
hub
->
GetLock
());
if
(
async_
)
st
=
SA_GotoAngleRelative_A
(
hub
->
getMCSHandle
(),
channel_
,
dAngle
,
dRev
,
holdTime
);
else
st
=
SA_GotoAngleRelative_S
(
hub
->
getMCSHandle
(),
channel_
,
dAngle
,
dRev
,
holdTime
);
if
(
st
!=
SA_OK
)
return
st
;
}
else
{
int
steps
=
static_cast
<
int
>
(
dAngT
);
if
(
steps
>
olMaxSteps
)
steps
=
olMaxSteps
;
if
(
steps
<
-
olMaxSteps
)
steps
=
-
olMaxSteps
;
MMThreadGuard
myLock
(
hub
->
GetLock
());
if
(
async_
)
st
=
SA_StepMove_A
(
hub
->
getMCSHandle
(),
channel_
,
steps
,
olAmplitude
,
olFrequency
);
else
st
=
SA_StepMove_S
(
hub
->
getMCSHandle
(),
channel_
,
steps
,
olAmplitude
,
olFrequency
);
if
(
st
!=
SA_OK
)
return
st
;
posSteps_
+=
steps
;
}
return
DEVICE_OK
;
}
// This function actually takes [uー] instead of [um]
int
SmaractRotStage
::
SetPositionUm
(
double
angT
)
{
SA_STATUS
st
;
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
if
(
hasSensor_
)
{
int
angT_int
=
static_cast
<
int
>
(
angT
);
unsigned
int
angle
;
int
tmp
=
angT_int
%
360000
;
signed
int
rev
=
angT
/
360000
;
if
(
tmp
>
0
)
angle
=
tmp
;
else
angle
=
tmp
+
360000
;
MMThreadGuard
myLock
(
hub
->
GetLock
());
if
(
async_
)
st
=
SA_GotoAngleAbsolute_A
(
hub
->
getMCSHandle
(),
channel_
,
angle
,
rev
,
holdTime
);
else
st
=
SA_GotoAngleAbsolute_S
(
hub
->
getMCSHandle
(),
channel_
,
angle
,
rev
,
holdTime
);
if
(
st
!=
SA_OK
)
return
st
;
}
else
{
int
steps
=
static_cast
<
int
>
(
angT
)
-
posSteps_
;
if
(
steps
>
olMaxSteps
)
steps
=
olMaxSteps
;
if
(
steps
<
-
olMaxSteps
)
steps
=
-
olMaxSteps
;
unsigned
int
amplitude
=
0
;
unsigned
int
freq
=
0
;
MMThreadGuard
myLock
(
hub
->
GetLock
());
if
(
async_
)
st
=
SA_StepMove_A
(
hub
->
getMCSHandle
(),
channel_
,
steps
,
olAmplitude
,
olFrequency
);
else
st
=
SA_StepMove_S
(
hub
->
getMCSHandle
(),
channel_
,
steps
,
olAmplitude
,
olFrequency
);
if
(
st
!=
SA_OK
)
return
st
;
posSteps_
=
steps
;
}
return
DEVICE_OK
;
}
int
SmaractRotStage
::
SetOrigin
()
{
SA_STATUS
st
;
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
MMThreadGuard
myLock
(
hub
->
GetLock
());
if
(
async_
)
st
=
SA_SetPosition_A
(
hub
->
getMCSHandle
(),
channel_
,
0
);
else
st
=
SA_SetPosition_S
(
hub
->
getMCSHandle
(),
channel_
,
0
);
if
(
st
!=
SA_OK
)
return
st
;
return
DEVICE_OK
;
}
int
SmaractRotStage
::
SetFrequency
(
int
f
)
{
SA_STATUS
st
;
unsigned
int
freq
=
static_cast
<
unsigned
int
>
(
f
);
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
MMThreadGuard
myLock
(
hub
->
GetLock
());
if
(
async_
)
st
=
SA_SetClosedLoopMaxFrequency_A
(
hub
->
getMCSHandle
(),
channel_
,
freq
);
else
st
=
SA_SetClosedLoopMaxFrequency_S
(
hub
->
getMCSHandle
(),
channel_
,
freq
);
if
(
st
!=
SA_OK
)
return
st
;
return
DEVICE_OK
;
}
int
SmaractRotStage
::
SetErrorReporting
(
boolean
reporting
)
{
return
DEVICE_UNSUPPORTED_COMMAND
;
}
// action interface
int
SmaractRotStage
::
OnLimit
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
return
DEVICE_UNSUPPORTED_COMMAND
;
}
int
SmaractRotStage
::
OnFrequency
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
return
DEVICE_UNSUPPORTED_COMMAND
;
}
int
SmaractRotStage
::
Move
(
double
velocity
)
{
return
DEVICE_UNSUPPORTED_COMMAND
;
}
int
SmaractRotStage
::
OnOLMove
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
SA_STATUS
st
;
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
if
(
!
hub
||
!
hub
->
IsConnected
())
{
return
ERR_HUB_NOT_CONNECTED
;
}
MMThreadGuard
myLock
(
hub
->
GetLock
());
if
(
eAct
==
MM
::
BeforeGet
)
{
/*double speed;
unsigned int stat;
SA_GetStatus_S(hub->getMCSHandle(), channel_, &stat);
if (stat == SA_STEPPING_STATUS)
{
speed = 0.0;
}
else
{
speed = 0.0;
}
pProp->Set(speed);*/
}
else
if
(
eAct
==
MM
::
AfterSet
)
{
double
vector
;
pProp
->
Get
(
vector
);
if
(
abs
(
vector
)
>
1.0
)
return
ERR_ASIGP_BAD_MULT
;
if
(
abs
(
vector
)
<
0.25
)
{
if
(
async_
)
st
=
SA_Stop_A
(
hub
->
getMCSHandle
(),
channel_
);
else
st
=
SA_Stop_S
(
hub
->
getMCSHandle
(),
channel_
);
if
(
st
!=
SA_OK
)
return
st
;
}
else
{
/*
* Recording the data
*/
long
recording
;
SA_STATUS
st
=
hub
->
GetProperty
(
"Recording"
,
recording
);
if
(
st
!=
SA_OK
)
return
st
;
if
(
recording
==
1
)
{
hub
->
Snapshot
();
/*
double recpos;
long speedLayer;
st = GetPositionUm(recpos);
if (st != SA_OK)
return st;
st = hub->GetProperty("speedLayer", speedLayer);
if (st != SA_OK)
return st;
if(recpos > minTravelRotational && recpos < maxTravelRotational)
{
std::string recordData = name_ + " "
+ std::to_string(static_cast<long long>(recpos)) + " "
+ std::to_string(static_cast<long long>(speedLayer));
hub->WriteToRecordingFile(recordData);
}
*/
}
int
stepsToMove
;
unsigned
int
amp_int
,
freq_int
;
double
amp_d
,
freq_d
,
steps_d
;
amp_d
=
static_cast
<
double
>
(
olAmplitude_
)
*
abs
(
vector
);
freq_d
=
static_cast
<
double
>
(
olFrequency_
)
*
abs
(
vector
);
steps_d
=
static_cast
<
double
>
(
olMaxSteps_
)
*
vector
;
amp_int
=
static_cast
<
unsigned
int
>
(
amp_d
);
freq_int
=
static_cast
<
unsigned
int
>
(
freq_d
);
stepsToMove
=
static_cast
<
int
>
(
steps_d
);
if
(
async_
)
{
if
(
hasSensor_
)
{
long
speedProp
;
SA_STATUS
st
=
hub
->
GetProperty
(
"speedLayer"
,
speedProp
);
if
(
st
!=
SA_OK
)
return
st
;
//Changed the speed
unsigned
int
speed
=
static_cast
<
unsigned
int
>
((
double
)
clvMax
*
abs
(
vector
)
*
hub
->
speedLayersValue
[
speedProp
]
/
2000
);
int
ang
;
signed
int
rev
;
if
(
vector
<
0
)
{
ang
=
minAng_
;
}
else
{
ang
=
maxAng_
;
}
st
=
SA_SetClosedLoopMoveSpeed_A
(
hub
->
getMCSHandle
(),
channel_
,
speed
);
if
(
st
!=
SA_OK
)
return
st
;
st
=
SA_GotoAngleAbsolute_A
(
hub
->
getMCSHandle
(),
channel_
,
ang
,
0
,
holdTime
);
if
(
st
!=
SA_OK
)
return
st
;
}
else
{
if
(
vector
<
0
)
st
=
SA_StepMove_A
(
hub
->
getMCSHandle
(),
channel_
,
-
olMaxSteps_
,
amp_int
,
freq_int
);
else
st
=
SA_StepMove_A
(
hub
->
getMCSHandle
(),
channel_
,
olMaxSteps_
,
amp_int
,
freq_int
);
if
(
st
!=
SA_OK
)
return
st
;
}
}
else
st
=
SA_StepMove_S
(
hub
->
getMCSHandle
(),
channel_
,
stepsToMove
,
amp_int
,
freq_int
);
if
(
st
!=
SA_OK
)
return
st
;
}
}
return
DEVICE_OK
;
}
ToolActuator
::
ToolActuator
()
:
name_
(
g_DeviceNameToolActuator
),
initialized_
(
false
),
portAvailable_
(
false
),
isMoving_
(
false
),
steps_
(
0
),
maxExtrude_
(
1400
)
{
SetErrorText
(
ERR_TACT_NOT_CONNECTED
,
g_Msg_ERR_TACT_NOT_CONNECTED
);
SetErrorText
(
ERR_TACT_ERR_COMMAND
,
g_Msg_ERR_TACT_ERR_COMMAND
);
InitializeDefaultErrorMessages
();
CreateProperty
(
MM
::
g_Keyword_Name
,
g_DeviceNameToolActuator
,
MM
::
String
,
true
);
CreateProperty
(
MM
::
g_Keyword_Description
,
"Arduino Stepper Tool Actuator"
,
MM
::
String
,
true
);
CPropertyAction
*
pAct
=
new
CPropertyAction
(
this
,
&
ToolActuator
::
OnPort
);
CreateProperty
(
MM
::
g_Keyword_Port
,
"Undefined"
,
MM
::
String
,
false
,
pAct
,
true
);
CreateProperty
(
"Direction"
,
"1"
,
MM
::
Integer
,
false
,
0
,
true
);
AddAllowedValue
(
"Direction"
,
"-1"
);
AddAllowedValue
(
"Direction"
,
"1"
);
pAct
=
new
CPropertyAction
(
this
,
&
ToolActuator
::
OnMove
);
CreateProperty
(
"OnMove"
,
"0"
,
MM
::
Float
,
false
,
pAct
);
SetPropertyLimits
(
"OnMove"
,
0.0
,
4500.0
);
UpdateProperty
(
"OnMove"
);
pAct
=
new
CPropertyAction
(
this
,
&
ToolActuator
::
OnStep
);
CreateProperty
(
"StepDir"
,
"0"
,
MM
::
Integer
,
false
,
pAct
);
AddAllowedValue
(
"StepDir"
,
"-1"
);
AddAllowedValue
(
"StepDir"
,
"1"
);
UpdateProperty
(
"StepDir"
);
}
int
ToolActuator
::
Initialize
()
{
if
(
initialized_
==
false
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
// The first second or so after opening the serial port, the Arduino is waiting for firmwareupgrades. Simply sleep 2 seconds.
CDeviceUtils
::
SleepMs
(
2000
);
// all conditions must be satisfied...
MM
::
DeviceDetectionStatus
result
=
MM
::
Misconfigured
;
char
answerTO
[
MM
::
MaxStrLength
];
std
::
string
portLowerCase
=
port_
;
for
(
std
::
string
::
iterator
its
=
portLowerCase
.
begin
();
its
!=
portLowerCase
.
end
();
++
its
)
{
*
its
=
(
char
)
tolower
(
*
its
);
}
if
(
0
<
portLowerCase
.
length
()
&&
0
!=
portLowerCase
.
compare
(
"undefined"
)
&&
0
!=
portLowerCase
.
compare
(
"unknown"
))
{
// record the default answer time out
GetCoreCallback
()
->
GetDeviceProperty
(
port_
.
c_str
(),
"AnswerTimeout"
,
answerTO
);
// device specific default communication parameters
GetCoreCallback
()
->
SetDeviceProperty
(
port_
.
c_str
(),
MM
::
g_Keyword_Handshaking
,
"Off"
);
GetCoreCallback
()
->
SetDeviceProperty
(
port_
.
c_str
(),
MM
::
g_Keyword_BaudRate
,
"9600"
);
GetCoreCallback
()
->
SetDeviceProperty
(
port_
.
c_str
(),
MM
::
g_Keyword_StopBits
,
"1"
);
GetCoreCallback
()
->
SetDeviceProperty
(
port_
.
c_str
(),
"AnswerTimeout"
,
"500.0"
);
GetCoreCallback
()
->
SetDeviceProperty
(
port_
.
c_str
(),
"DelayBetweenCharsMs"
,
"0"
);
MM
::
Device
*
pS
=
GetCoreCallback
()
->
GetDevice
(
this
,
port_
.
c_str
());
pS
->
Initialize
();
// The first second or so after opening the serial port, the Arduino is waiting for firmwareupgrades. Simply sleep 2 seconds.
CDeviceUtils
::
SleepMs
(
2000
);
PurgeComPort
(
port_
.
c_str
());
int
ret
=
SendSerialCommand
(
port_
.
c_str
(),
(
const
char
*
)
CMD_NME
,
"
\n
"
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
std
::
string
answer
;
ret
=
GetSerialAnswer
(
port_
.
c_str
(),
"
\n
"
,
answer
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
if
(
strncmp
(
answer
.
c_str
(),
NAME
,
3
)
!=
0
)
return
ret
;
//pS->Shutdown();
// always restore the AnswerTimeout to the default
//GetCoreCallback()->SetDeviceProperty(port_.c_str(), "AnswerTimeout", answerTO);
}
int
ret
=
SetPositionSteps
(
0
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
ret
=
GetPositionSteps
(
steps_
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
initialized_
=
true
;
hub
->
SetToolActuator
(
this
);
return
DEVICE_OK
;
}
}
int
ToolActuator
::
Shutdown
()
{
if
(
initialized_
)
initialized_
=
false
;
return
DEVICE_OK
;
}
int
ToolActuator
::
OnPort
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
pAct
)
{
if
(
pAct
==
MM
::
BeforeGet
)
{
pProp
->
Set
(
port_
.
c_str
());
}
else
if
(
pAct
==
MM
::
AfterSet
)
{
pProp
->
Get
(
port_
);
portAvailable_
=
true
;
}
return
DEVICE_OK
;
}
void
ToolActuator
::
GetName
(
char
*
name
)
const
{
CDeviceUtils
::
CopyLimitedString
(
name
,
name_
.
c_str
());
}
std
::
string
ToolActuator
::
ExportName
()
{
return
name_
;
}
bool
ToolActuator
::
Busy
()
{
if
(
isMoving_
==
true
)
// Query the Serial port one time to see if movement completed
{
std
::
string
command
=
CMD_MOQ
;
SendSerialCommand
(
port_
.
c_str
(),
command
.
c_str
(),
"
\n
"
);
std
::
string
answer
;
GetSerialAnswer
(
port_
.
c_str
(),
"
\n
"
,
answer
);
if
(
strncmp
(
answer
.
c_str
(),
STA_MOV
,
3
)
==
0
)
isMoving_
=
true
;
else
if
(
strncmp
(
answer
.
c_str
(),
STA_IDL
,
3
)
==
0
)
isMoving_
=
false
;
}
return
isMoving_
;
}
/*
* If the actuator is busy, ignore the command
*/
int
ToolActuator
::
SetPositionSteps
(
long
steps
)
{
if
(
Busy
())
{
return
DEVICE_OK
;
}
else
{
std
::
stringstream
cmd_str
;
cmd_str
<<
CMD_MOV
<<
" "
<<
steps
;
const
char
*
command
=
(
const
char
*
)
cmd_str
.
str
().
c_str
();
int
ret
=
SendSerialCommand
(
port_
.
c_str
(),
cmd_str
.
str
().
c_str
(),
"
\n
"
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
std
::
string
answer
;
ret
=
GetSerialAnswer
(
port_
.
c_str
(),
"
\n
"
,
answer
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
if
(
strncmp
(
answer
.
c_str
(),
STA_COK
,
3
)
!=
0
)
return
ERR_TACT_ERR_COMMAND
;
else
steps_
+=
steps
;
isMoving_
=
true
;
return
DEVICE_OK
;
}
}
int
ToolActuator
::
GetPositionSteps
(
long
&
steps
)
{
if
(
Busy
())
{
steps
=
steps_
;
return
DEVICE_OK
;
}
else
{
std
::
string
command
=
CMD_POQ
;
int
ret
=
SendSerialCommand
(
port_
.
c_str
(),
command
.
c_str
(),
"
\n
"
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
std
::
string
answer
;
ret
=
GetSerialAnswer
(
port_
.
c_str
(),
"
\n
"
,
answer
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
steps
=
stol
(
answer
);
return
DEVICE_OK
;
}
}
int
ToolActuator
::
GetPositionUm
(
double
&
pos
)
{
long
steps
;
int
ret
=
GetPositionSteps
(
steps
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
pos
=
static_cast
<
double
>
(
steps
)
*
UM_PER_STEP
;
return
DEVICE_OK
;
}
int
ToolActuator
::
SetPositionUm
(
double
pos
)
{
long
steps
=
static_cast
<
long
>
(
pos
*
STEPS_PER_UM
);
int
ret
=
SetPositionSteps
(
steps
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
return
DEVICE_OK
;
}
int
ToolActuator
::
SetRelativePositionUm
(
double
pos
)
{
int
ret
=
SetPositionUm
(
pos
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
return
DEVICE_OK
;
}
int
ToolActuator
::
OnMove
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
{
}
else
if
(
eAct
==
MM
::
AfterSet
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
double
vector
;
pProp
->
Get
(
vector
);
if
(
abs
(
vector
)
>
1.0
)
return
ERR_ASIGP_BAD_MULT
;
double
pos_steps
;
pos_steps
=
static_cast
<
double
>
(
maxExtrude_
)
*
vector
;
int
pos_steps_int
=
static_cast
<
int
>
(
pos_steps
);
/*
Recording code
*/
long
recording
;
SA_STATUS
st
=
hub
->
GetProperty
(
"Recording"
,
recording
);
if
(
st
!=
SA_OK
)
return
st
;
if
(
recording
==
1
)
{
hub
->
Snapshot
();
/*
std::string recordData = name_ + " "
+ std::to_string(static_cast<long long>(pos_steps_int));
hub->WriteToRecordingFile(recordData);
*/
}
std
::
stringstream
cmd_str
;
cmd_str
<<
CMD_MOV
<<
" "
<<
pos_steps_int
;
const
char
*
command
=
(
const
char
*
)
cmd_str
.
str
().
c_str
();
int
ret
=
SendSerialCommand
(
port_
.
c_str
(),
(
const
char
*
)
cmd_str
.
str
().
c_str
(),
"
\n
"
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
std
::
string
answer
;
ret
=
GetSerialAnswer
(
port_
.
c_str
(),
"
\n
"
,
answer
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
if
(
strncmp
(
answer
.
c_str
(),
STA_ERR
,
3
)
==
0
)
{
return
ERR_TACT_ERR_COMMAND
;
}
if
(
strncmp
(
answer
.
c_str
(),
STA_OOR
,
3
)
==
0
)
{
return
ERR_TACT_ERR_OUT_OF_RANGE
;
}
}
return
DEVICE_OK
;
}
int
ToolActuator
::
OnStep
(
MM
::
PropertyBase
*
pProp
,
MM
::
ActionType
eAct
)
{
if
(
eAct
==
MM
::
BeforeGet
)
{
}
else
if
(
eAct
==
MM
::
AfterSet
)
{
SmaractHub
*
hub
=
static_cast
<
SmaractHub
*>
(
GetParentHub
());
long
dir
;
pProp
->
Get
(
dir
);
steps_
+=
dir
;
if
(
steps_
<
0
)
steps_
=
0
;
else
if
(
steps_
>
14
)
steps_
=
14
;
double
pos_steps
;
pos_steps
=
static_cast
<
double
>
(
steps_
*
STEPSIZE
);
int
pos_steps_int
=
static_cast
<
int
>
(
pos_steps
);
/*
Recording code
*/
long
recording
;
SA_STATUS
st
=
hub
->
GetProperty
(
"Recording"
,
recording
);
if
(
st
!=
SA_OK
)
return
st
;
if
(
recording
==
1
)
{
hub
->
Snapshot
();
/*
std::string recordData = name_ + " "
+ std::to_string(static_cast<long long>(pos_steps_int));
hub->WriteToRecordingFile(recordData);
*/
}
std
::
stringstream
cmd_str
;
cmd_str
<<
CMD_MOV
<<
" "
<<
pos_steps_int
;
const
char
*
command
=
cmd_str
.
str
().
c_str
();
int
ret
=
SendSerialCommand
(
port_
.
c_str
(),
(
const
char
*
)
cmd_str
.
str
().
c_str
(),
"
\n
"
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
std
::
string
answer
;
ret
=
GetSerialAnswer
(
port_
.
c_str
(),
"
\n
"
,
answer
);
if
(
ret
!=
DEVICE_OK
)
return
ret
;
if
(
strncmp
(
answer
.
c_str
(),
STA_ERR
,
3
)
==
0
)
{
return
ERR_TACT_ERR_COMMAND
;
}
if
(
strncmp
(
answer
.
c_str
(),
STA_OOR
,
3
)
==
0
)
{
return
ERR_TACT_ERR_OUT_OF_RANGE
;
}
}
return
DEVICE_OK
;
}
Event Timeline
Log In to Comment