Homec4science
Diffusion uSurgery main dll file (master) Browse

usurgerydll/Code/SmaractHub.cpp
0731d03cb8e7master

Last Change
Raw 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;
}
c4science · Help