diff --git a/SmaractHub.cpp b/SmaractHub.cpp new file mode 100644 index 0000000..758cc7b --- /dev/null +++ b/SmaractHub.cpp @@ -0,0 +1,3422 @@ +#include "SmaractHub.h" +#include "ModuleInterface.h" +#include +#include +#include +#include +#include +#include + +#define _USE_MATH_DEFINES +#include + +#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_DEFAULT "1000" +#define MAXSPEEDLAYER 1000 + +#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), + 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; + 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 + bool alphaknown = false; + bool betaknown = false; + bool xknown = false; + bool yknown = false; + bool zknown = false; + + //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 = true; + else + alphaknown = false; + + //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 = true; + else + betaknown = false; + + //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 = true; + else + xknown = false; + + //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 = true; + else + yknown = false; + + //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 = true; + else + zknown = false; + + // 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; + + + + + initialized_ = true; + } + + // THIS IS NOT A LONG-TERM PROPERTY. TODO : REMOVE + /* + pAct = new CPropertyAction(this, &SmaractHub::ChangeScale); + CreateProperty("ReScale", PROPERTY_TOGGLE_DEFAULT, MM::Integer, false, pAct); + AddAllowedValue("ReScale", PROPERTY_TOGGLE_ON); + AddAllowedValue("ReScale", PROPERTY_TOGGLE_OFF); + UpdateProperty("ReScale"); + */ + + 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"); + + CreateProperty("isRotOn", PROPERTY_TOGGLE_DEFAULT, MM::Integer, false, 0); + AddAllowedValue("isRotOn", PROPERTY_TOGGLE_ON); + AddAllowedValue("isRotOn", PROPERTY_TOGGLE_OFF); + UpdateProperty("isRotOn"); + + pAct = new CPropertyAction(this, &SmaractHub::OnSpeedLayerChange); + CreateProperty("speedLayer", PROPERTY_SPEED_LAYER_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"); + + + + 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::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);*/ + + return st; +} + +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; + + if(rotOn && xROTStage) + { + double vect; + pProp->Get(vect); + 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; + + 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; + + 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; + + 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(1900 + ltm->tm_year)) + "-" + +std::to_string(static_cast(1 + ltm->tm_mon)) + "-" + + std::to_string(static_cast(ltm->tm_mday)) + "-" + + std::to_string(static_cast(ltm->tm_hour)) + "h" + + std::to_string(static_cast(ltm->tm_min)) + "m" + + std::to_string(static_cast(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 = ""; + + xTRStage->GetPositionUm(pos); + name = "X"; + recordData += name + " " + std::to_string(static_cast(pos)) + " "; + + yTRStage->GetPositionUm(pos); + name = "Y"; + recordData += name + " " + std::to_string(static_cast(pos)) + " "; + + zTRStage->GetPositionUm(pos); + name = "Z"; + recordData += name + " " + std::to_string(static_cast(pos)) + " "; + + xROTStage->GetPositionUm(pos); + name = "Alpha"; + recordData += name + " " + std::to_string(static_cast(pos)) + " "; + + yROTStage->GetPositionUm(pos); + name = "Beta"; + recordData += name + " " + std::to_string(static_cast(pos)) + " "; + + toolActuator->GetPositionSteps(toolpos); + name = toolActuator->ExportName(); + recordData += name + " " + std::to_string(static_cast(toolpos)) + " "; + + recordData += "Speedlayer " + std::to_string(static_cast(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(pos)) + + " " + std::to_string(static_cast(speedLayer)); + + WriteToRecordingFile(recordData); + + return SA_OK; +} + +int SmaractHub::RecordTool(long pos, std::string name) +{ + std::string recordData = name + " " + std::to_string(static_cast(pos)); + + WriteToRecordingFile(recordData); + + return SA_OK; +} + +/* Takes the data string and writes it in the previously created file */ +void SmaractHub::WriteToRecordingFile(std::string input) +{ + if(newFile == true) + { + newFile = false; + recordingFile << input; + } + recordingFile << '\n' + 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) +{ + 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 + { + // 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; + } + + // 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 + 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 + 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 + 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; + + 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(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(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(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(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) +{ + 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); + 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); + + 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; + + while(!readingFile.eof()) + { + std::getline(readingFile,readLine); + //DecodeAndAct(readLine); + } + + 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(); + + if(verbose) + MessageBox(nullptr, TEXT( "Recorded file movement completed" ), TEXT( "Message" ), MB_OK | MB_ICONINFORMATION | MB_SETFOREGROUND); + + return SA_OK; +} + +void SmaractHub::DecodeAndAct(std::string const& line) +{ + + long long timeout = 0; + long long ctimeout = 0; + 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); + } + + strIndex1 = line.find(stages[6]); + stringStream.str(line.substr(strIndex1)); + stringStream >> stringSpeed; + stringStream >> stringSpeed; + speedLayer = std::stod(stringSpeed); + + 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(distance[i])*1000)/static_cast(speed[i]); + if(ctimeout > timeout) + { + timeout = ctimeout; + index = i; + } + } + + if(timeout > MAX_REPLAY_TIMEOUT) + { + speed[index] = 1000*static_cast(distance[index])/MAX_REPLAY_TIMEOUT; + timeout = MAX_REPLAY_TIMEOUT; + if(speed[index] > MAX_REPLAY_SPEED) + { + speed[index] = MAX_REPLAY_SPEED; + timeout = (static_cast(distance[index])*1000)/static_cast(speed[index]); //ms + } + } + + if(timeout == 0) + timeout = 1; + + for(i = 0; i <= 4; i++) + { + speed[i] = 1000*static_cast(distance[i])/timeout; + st = SA_SetClosedLoopMoveSpeed_A(getMCSHandle(), i, speed[i]); + if (st != SA_OK) + return; + } + + timeout += 1; //adding a 1 ms margin + + + 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(position[i]), holdTime); + } + + for(i = 3; i <= 4; i++) + { + SA_GotoAngleAbsolute_A(getMCSHandle(), i, static_cast(position[i]), 0, holdTime); + } + + + toolActuator->SetPositionSteps(static_cast(position[5])); + + + + CDeviceUtils::SleepMs(timeout); + 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(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(distance)*1000)/static_cast(speed); //ms + + if(timeout > MAX_REPLAY_TIMEOUT) + { + speed = 1000*static_cast(distance)/MAX_REPLAY_TIMEOUT; + timeout = MAX_REPLAY_TIMEOUT; + if(speed > MAX_REPLAY_SPEED) + { + speed = MAX_REPLAY_SPEED; + timeout = (static_cast(distance)*1000)/static_cast(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(position), holdTime); + else + SA_GotoAngleAbsolute_A(getMCSHandle(), tempChannel, static_cast(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(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(pos); + return DEVICE_OK; +}*/ + +int SmaractTrStage::Initialize() +{ + if (initialized_ == false) + { + SA_STATUS st; + SmaractHub* hub = static_cast(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(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(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(posMin); + pMax = static_cast(posMax); + return DEVICE_OK; +} + +int SmaractTrStage::GetPositionUm(double& pos) +{ + SmaractHub* hub = static_cast(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(position); + return DEVICE_OK; +} + +int SmaractTrStage::SetPositionUm(double pos) +{ + SA_STATUS st; + SmaractHub* hub = static_cast(GetParentHub()); + if (!hub || !hub->IsConnected()) + { + return ERR_HUB_NOT_CONNECTED; + } + // Here the value is converted to mm + signed int position = static_cast(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(GetParentHub()); + if (!hub || !hub->IsConnected()) + return ERR_HUB_NOT_CONNECTED; + + // Here the value is converted to mm + signed int dPosition = static_cast(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(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(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(f); + SmaractHub* hub = static_cast(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(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(recpos)) + " " + + std::to_string(static_cast(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((double)clvMax * abs(vector)*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(olAmplitude_) * abs(vector); + freq_d = static_cast(olFrequency_) * abs(vector); + steps_d = static_cast(olMaxSteps_) * vector; + + amp_int = static_cast(amp_d); + freq_int = static_cast(freq_d); + stepsToMove = static_cast(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(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(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(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(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(GetParentHub()); + if (!hub || !hub->IsConnected()) + return ERR_HUB_NOT_CONNECTED; + unsigned int speed = static_cast((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(GetParentHub()); + if (!hub || !hub->IsConnected()) + return ERR_HUB_NOT_CONNECTED; + unsigned int speed = static_cast((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(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(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(angle) + 360000000 * static_cast(revolution)); + } + else + { + angT = static_cast(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(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(minAng) + 2.0 * M_PI * static_cast(minRev) * 1000.0; + maxAngT = static_cast(maxAng) + 2.0 * M_PI * static_cast(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(GetParentHub()); + if (!hub || !hub->IsConnected()) + { + return ERR_HUB_NOT_CONNECTED; + } + + if (hasSensor_) + { + int dAngT_int = static_cast(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(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(GetParentHub()); + if (!hub || !hub->IsConnected()) + { + return ERR_HUB_NOT_CONNECTED; + } + + if (hasSensor_) + { + int angT_int = static_cast(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(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(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(f); + SmaractHub* hub = static_cast(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(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(recpos)) + " " + + std::to_string(static_cast(speedLayer)); + hub->WriteToRecordingFile(recordData); + } + */ + + + } + + int stepsToMove; + unsigned int amp_int, freq_int; + double amp_d, freq_d, steps_d; + amp_d = static_cast(olAmplitude_) * abs(vector); + freq_d = static_cast(olFrequency_) * abs(vector); + steps_d = static_cast(olMaxSteps_) * vector; + + amp_int = static_cast(amp_d); + freq_int = static_cast(freq_d); + stepsToMove = static_cast(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((double)clvMax * abs(vector)*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_(2000) +{ + 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(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(steps) * UM_PER_STEP; + return DEVICE_OK; +} + +int ToolActuator::SetPositionUm(double pos) +{ + long steps = static_cast(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(GetParentHub()); + double vector; + pProp->Get(vector); + if (abs(vector) > 1.0) + return ERR_ASIGP_BAD_MULT; + + double pos_steps; + pos_steps = static_cast(maxExtrude_) * vector; + + int pos_steps_int = static_cast(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(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(GetParentHub()); + + long dir; + pProp->Get(dir); + + steps_ += dir; + if(steps_ < 0) + steps_ = 0; + else if(steps_ > 20) + steps_ = 20; + + double pos_steps; + pos_steps = static_cast(steps_ * STEPSIZE); + + int pos_steps_int = static_cast(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(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; +} \ No newline at end of file diff --git a/SmaractHub.h b/SmaractHub.h new file mode 100644 index 0000000..8350c2a --- /dev/null +++ b/SmaractHub.h @@ -0,0 +1,291 @@ +#pragma once + +#include "MMDevice.h" +#include "DeviceBase.h" +#include "../../3rdparty/MCSControl.h" +#include +#include +#include +#include + +/* + * Global custom error codes and messages + */ +#define ERR_HUB_NOT_CONNECTED 10001 +#define ERR_HUB_NONE_DETECTED 10002 +#define ERR_ASIGP_BAD_MULT 10003 +#define ERR_UNEXPECTED_PACKET 10004 +#define ERR_DEVICE_NOT_FOUND 10005 +#define ERR_NO_FILE_FOUND 10006 +#define ERR_NOT_REFERENCED 10007 + + + +const char* const g_Msg_ERR_HUB_NOT_CONNECTED = + "Hub Device not found. The Smaract Hub device is needed to create this device"; +const char* const g_Msg_ERR_HUB_NONE_DETECTED = + "No Hub device could be detected"; +const char* const g_Msg_ERR_ASIGP_BAD_MULT = + "ASI Gamepad bad multiplier (> 1 or < -1)"; +const char* const g_Msg_ERR_UNEXPECTED_PACKET = + "Unexpected packet received"; + +class SmaractTrStage; +class SmaractRotStage; +class ToolActuator; + +class SmaractHub : public HubBase +{ +public: + SmaractHub(); + ~SmaractHub(); + + int Initialize(); + int Shutdown(); + void GetName(char* pszName) const; + bool Busy(); + + MM::DeviceDetectionStatus DetectDevice(void); + int DetectInstalledDevices(); + + bool IsConnected(); + + unsigned int getChannel(const char* devName); + bool isCommModeAsync(); + bool isCalibrate(); + bool isReference(); + bool isZStageLocked(); + bool OpenNewFile(); + void WriteToRecordingFile(std::string inputString); + void CloseRecordingFile(); + int ChangeScale(); + int MoveTRStages(); + int Snapshot(); + int RecordStage(double pos, long speedLayer, std::string name); + int RecordTool(long pos, std::string name); + int PlayRecording(MM::PropertyBase* pProp, MM::ActionType eAct); + int OpenExistingFile(char* fileName); + void DecodeAndAct(std::string const& line); + void SetTRStage(const char* name, SmaractTrStage* pStage); + void SetRotStage(const char* name, SmaractRotStage* pStage); + void SetToolActuator(ToolActuator* pStage); + int Reference(MM::PropertyBase* pProp, MM::ActionType eAct); + + SA_INDEX getMCSHandle(); + + static MMThreadLock& GetLock() {return lock_;} + +private: + int OnCommMode(MM::PropertyBase* pProp, MM::ActionType eAct); + bool async_; + + int OnCalibrate(MM::PropertyBase* pProp, MM::ActionType eAct); + bool calibrate_; + + int OnReference(MM::PropertyBase* pProp, MM::ActionType eAct); + bool reference_; + + int OnXOLMove(MM::PropertyBase* pProp, MM::ActionType eAct); + int OnYOLMove(MM::PropertyBase* pProp, MM::ActionType eAct); + int OnZOLMove(MM::PropertyBase* pProp, MM::ActionType eAct); + int OnToolMove(MM::PropertyBase* pProp, MM::ActionType eAct); + int OnSpeedLayerChange(MM::PropertyBase* pProp, MM::ActionType eAct); + int OnRecordingToggle(MM::PropertyBase* pProp, MM::ActionType eAct); + + SmaractTrStage* xTRStage; + SmaractTrStage* yTRStage; + SmaractTrStage* zTRStage; + SmaractRotStage* xROTStage; + SmaractRotStage* yROTStage; + SmaractRotStage* zROTStage; + ToolActuator* toolActuator; + + int lastPositions [5]; + bool newFile; + + bool initialized_; + bool isConnected_; + bool hasRecordedOnce_; + unsigned int noChannels_; + SA_INDEX mcsHandle_; + std::string port_; + std::string name_; + std::vector devConn_; + std::vector peripherals_; + static MMThreadLock lock_; + std::ofstream recordingFile; + std::ifstream readingFile; +}; + + +class SmaractTrStage : public CStageBase +{ +public: + SmaractTrStage(const char* devName); + ~SmaractTrStage(); + + int Initialize(); + int Shutdown(); + void GetName(char* pszName) const; + bool Busy(); + + int IsStageSequenceable(bool& isSequenceable) const {isSequenceable = false; return DEVICE_OK;} + bool IsContinuousFocusDrive() const {return false;} + + int GetPositionUm(double& pos); + int GetPositionSteps(long &steps) { return DEVICE_UNSUPPORTED_COMMAND; } + int GetLimits(double& /*min*/, double& /*max*/); + + int SetPositionUm(double pos); + int SetRelativePositionUm(double pos); + int SetPositionSteps(long steps) { return DEVICE_UNSUPPORTED_COMMAND; } + + int SetOrigin(); + int SetFrequency(int x); + int SetErrorReporting(boolean reporting); + + int OnLimit(MM::PropertyBase* pProp, MM::ActionType eAct); + int OnFrequency(MM::PropertyBase* pProp, MM::ActionType eAct); + int Move(double velocity); + int OnOLMove(MM::PropertyBase* pProp, MM::ActionType eAct); + + std::string ExportName(); + + //TO REMOVE - Testing + void testMoveCallback(); + +private: + unsigned int channel_; + bool async_; + bool hasSensor_; + bool initialized_; + std::string name_; + int olMoveScale_; + int OnOLStep(MM::PropertyBase* pProp, MM::ActionType eAct); + signed int minPos_; // [nm] + signed int maxPos_; // [nm] + unsigned int olAmplitude_; + unsigned int olFrequency_; + int olMaxSteps_; +}; + +class SmaractRotStage : public CStageBase +{ +public: + SmaractRotStage(const char* devName); + ~SmaractRotStage(); + + int Initialize(); + int Shutdown(); + void GetName(char* pszName) const; + bool Busy(); + + int IsStageSequenceable(bool& isSequenceable) const {isSequenceable = false; return DEVICE_OK;} + bool IsContinuousFocusDrive() const {return false;} + + int GetPositionUm(double& angT /*[u°]*/); + int GetPositionSteps(long &steps) { return DEVICE_UNSUPPORTED_COMMAND; } + int GetLimits(double& minAngT /*[u°]*/, double& maxAngT /*[u°]*/); + + int SetPositionUm(double angT /*[u°]*/); + int SetRelativePositionUm(double dAngT /*[u°]*/); + int SetPositionSteps(long steps) { return DEVICE_UNSUPPORTED_COMMAND; } + + int SetOrigin(); + int SetFrequency(int x); + int SetErrorReporting(boolean reporting); + + int OnLimit(MM::PropertyBase* pProp, MM::ActionType eAct); + int OnFrequency(MM::PropertyBase* pProp, MM::ActionType eAct); + int Move(double velocity); + int OnOLMove(MM::PropertyBase* pProp, MM::ActionType eAct); + + std::string ExportName(); + +private: + unsigned int channel_; + bool hasSensor_; + bool initialized_; + bool async_; + std::string name_; + long posSteps_; // Step recorder for rotary stage without sensor + int olMoveScale_; + signed int minAng_; // [u°] + signed int maxAng_; // [u°] + unsigned int olAmplitude_; + unsigned int olFrequency_; + int olMaxSteps_; +}; + + +// Arduino Tool Actuator +#define ERR_TACT_NOT_CONNECTED 20001 +#define ERR_TACT_ERR_COMMAND 20002 +#define ERR_TACT_ERR_OUT_OF_RANGE 20003 + +const char* const g_Msg_ERR_TACT_NOT_CONNECTED = + "Tool Actuator Arduino not found"; +const char* const g_Msg_ERR_TACT_ERR_COMMAND = + "Tool Actuator Arduino command error"; + +#define UM_PER_STEP 20 +#define STEPS_PER_UM 0.05 +#define STEPSIZE 100 + +#define BUF_LEN (64) +#define NAME ("ATA") +#define CMD_NME ("NME") // Get controller name +#define CMD_MOV ("MOV") // Move to +#define CMD_MOQ ("MOQ") // Ask movement status +#define CMD_HOM ("HOM") // Homing +#define CMD_POS ("POS") // Set position +#define CMD_POQ ("POQ") // Get position +#define CMD_DLY ("DLY") // Set step delay +#define CMD_DLQ ("DLQ") // Get step delay +#define STA_ERR ("ERR") // Return error if command is not understood +#define STA_COK ("COK") // Return "command OK" if command is understood +#define STA_OOR ("OOR") // Return "position out of reach" +#define STA_MOV ("MOV") // Return "MOV" when movement finished +#define STA_IDL ("IDL") // Return "IDL" when movement finished + +class ToolActuator : public CStageBase +{ +public: + ToolActuator(); + ~ToolActuator() {}; + + int Initialize(); + int Shutdown(); + void GetName(char* pszName) const; + bool Busy(); + + std::string ExportName(); + + int IsStageSequenceable(bool& isSequenceable) const { isSequenceable = false; return DEVICE_OK;} + bool IsContinuousFocusDrive() const { return false; } + + int GetPositionUm(double &pos); + int GetPositionSteps(long &steps); + int GetLimits(double& , double& ) { return DEVICE_UNSUPPORTED_COMMAND; } + + int SetPositionUm(double pos); + int SetRelativePositionUm(double pos); + int SetPositionSteps(long steps); + + int SetOrigin() { return DEVICE_UNSUPPORTED_COMMAND; } + + int OnMove(MM::PropertyBase* pProp, MM::ActionType eAct); + int OnPort(MM::PropertyBase* pPropt, MM::ActionType eAct); + int OnStep(MM::PropertyBase* pProp, MM::ActionType eAct); + + +private: + std::string name_; + std::string port_; + bool initialized_; + bool portAvailable_; + bool isMoving_; + long steps_; + int maxExtrude_; // [steps] + +}; \ No newline at end of file diff --git a/SmaractTTTRRR.vcxproj b/SmaractTTTRRR.vcxproj new file mode 100644 index 0000000..2e64b10 --- /dev/null +++ b/SmaractTTTRRR.vcxproj @@ -0,0 +1,159 @@ + + + + + Debug + Win32 + + + Debug + x64 + + + Release + Win32 + + + Release + x64 + + + + {6656A29F-832B-4AF5-97B3-9DE849C97041} + Win32Proj + SmaractTTTRRR + + + + DynamicLibrary + true + Unicode + Windows7.1SDK + + + DynamicLibrary + true + Unicode + Windows7.1SDK + + + DynamicLibrary + false + true + Unicode + Windows7.1SDK + + + DynamicLibrary + false + true + Unicode + Windows7.1SDK + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + true + + + false + + + false + + + + Disabled + WIN32;_DEBUG;_WINDOWS;_USRDLL;SMARACTTTTRRR_EXPORTS;%(PreprocessorDefinitions) + + + Windows + true + MCSControl.lib;%(AdditionalDependencies) + C:\Users\MICROBS2\Documents\2018_RoboticManipulation\microManagerDev\3rdparty;%(AdditionalLibraryDirectories) + + + + + Disabled + WIN32;_DEBUG;_WINDOWS;_USRDLL;SMARACTTTTRRR_EXPORTS;%(PreprocessorDefinitions) + + + Windows + true + MCSControl.lib;%(AdditionalDependencies) + C:\Users\MICROBS2\Documents\2018_RoboticManipulation\microManagerDev\3rdparty;%(AdditionalLibraryDirectories) + + + + + MaxSpeed + true + true + WIN32;NDEBUG;_WINDOWS;_USRDLL;SMARACTTTTRRR_EXPORTS;%(PreprocessorDefinitions) + + + Windows + true + true + true + MCSControl.lib;%(AdditionalDependencies) + C:\Users\MICROBS2\Documents\2018_RoboticManipulation\microManagerDev\3rdparty;%(AdditionalLibraryDirectories) + + + + + MaxSpeed + true + true + WIN32;NDEBUG;_WINDOWS;_USRDLL;SMARACTTTTRRR_EXPORTS;%(PreprocessorDefinitions) + + + Windows + true + true + true + MCSControl.lib;%(AdditionalDependencies) + C:\Users\MICROBS2\Documents\2018_RoboticManipulation\microManagerDev\3rdparty;%(AdditionalLibraryDirectories) + + + + + + + + + + + {b8c95f39-54bf-40a9-807b-598df2821d55} + + + + + + \ No newline at end of file diff --git a/SmaractTTTRRR.vcxproj.filters b/SmaractTTTRRR.vcxproj.filters new file mode 100644 index 0000000..5f25332 --- /dev/null +++ b/SmaractTTTRRR.vcxproj.filters @@ -0,0 +1,27 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hpp;hxx;hm;inl;inc;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Header Files + + + + + Source Files + + + \ No newline at end of file diff --git a/SmaractTTTRRR.vcxproj.user b/SmaractTTTRRR.vcxproj.user new file mode 100644 index 0000000..ace9a86 --- /dev/null +++ b/SmaractTTTRRR.vcxproj.user @@ -0,0 +1,3 @@ + + + \ No newline at end of file