bool RAMPSXYStage::Busy()
{
LogMessage("XYStage: Busy called");
RAMPSHub* pHub = static_cast<RAMPSHub*>(GetParentHub());
std::string status = pHub->GetState();
LogMessage("Status is:");
LogMessage(status);
status_ = status;
if (status_ == "Idle") {
LogMessage("idle, return false.");
return false;
}
else if (status_ == "Running") {
LogMessage("Running, return true.");
return true;
}
else {
LogMessage("Unexpected status.");
return false;
}
}
int RAMPSXYStage::SetPositionSteps(long x, long y)
{
LogMessage("XYStage: SetPositionSteps");
RAMPSHub* pHub = static_cast<RAMPSHub*>(GetParentHub());
std::string status = pHub->GetState();
if (status == "Running") {
return ERR_STAGE_MOVING;
}
double newPosX = x * stepSize_um_;
double newPosY = y * stepSize_um_;
double difX = newPosX - posX_um_;
double difY = newPosY - posY_um_;
double distance = sqrt( (difX * difX) + (difY * difY) );
posX_um_ = x * stepSize_um_;
posY_um_ = y * stepSize_um_;
// TODO(dek): if no position change, don't send new position.
char buff[100];
sprintf(buff, "G0 X%f Y%f", posX_um_/1000., posY_um_/1000.);
std::string buffAsStdStr = buff;
int ret = pHub->SendCommand(buffAsStdStr);
if (ret != DEVICE_OK)
return ret;
pHub->SetTargetXY(posX_um_, posY_um_);
ret = OnXYStagePositionChanged(posX_um_, posY_um_);
if (ret != DEVICE_OK)
return ret;
return DEVICE_OK;
}
/*
* Requests movement to new z postion from the controller. This function does the actual communication
*/
int RAMPSZStage::SetPositionSteps(long steps)
{
RAMPSHub* pHub = static_cast<RAMPSHub*>(GetParentHub());
if (pHub->Busy()) {
return ERR_STAGE_MOVING;
}
posZ_um_ = steps * stepSize_um_;
char buff[100];
sprintf(buff, "G0 Z%f", posZ_um_/1000.);
std::string buffAsStdStr = buff;
int ret = pHub->SendCommand(buffAsStdStr);
if (ret != DEVICE_OK)
return ret;
std::string answer;
ret = pHub->ReadResponse(answer, 1000);
if (ret != DEVICE_OK) {
LogMessage("Error sending Z move.");
return ret;
}
if (answer != "ok") {
LogMessage("Failed to get ok response to Z move.");
}
ret = OnStagePositionChanged(posZ_um_);
if (ret != DEVICE_OK)
return ret;
return DEVICE_OK;
}
/*
* Requests current z postion from the controller. This function does the actual communication
*/
int RAMPSZStage::GetPositionSteps(long& steps)
{
RAMPSHub* pHub = static_cast<RAMPSHub*>(GetParentHub());
pHub->GetStatus();
steps = (long)(posZ_um_ / stepSize_um_);
// TODO(dek): implement status to get Z position
return DEVICE_OK;
}
int RAMPSHub::SetAcceleration(double acceleration) {
RAMPSHub* pHub = static_cast<RAMPSHub*>(GetParentHub());
std::string accStr = CDeviceUtils::ConvertToString(acceleration);
std::string command = "M201 X" + accStr + " Y" + accStr + " Z" + accStr;
std::string result;
PurgeComPortH();
int ret = pHub->SendCommand(command);
if (ret != DEVICE_OK) return ret;
ret = pHub->ReadResponse(result);
if (ret != DEVICE_OK) return ret;
if (result != "ok") {
LogMessage("Expected OK");
}
return ret;
}
int RAMPSHub::SetVelocity(double x, double y, double z) {
RAMPSHub* pHub = static_cast<RAMPSHub*>(GetParentHub());
std::string xStr = CDeviceUtils::ConvertToString(x);
std::string yStr = CDeviceUtils::ConvertToString(y);
std::string zStr = CDeviceUtils::ConvertToString(z);
std::string command = "M203 X" + xStr + " Y" + yStr + " Z" + zStr;
std::string result;
PurgeComPortH();
int ret = pHub->SendCommand(command);
if (ret != DEVICE_OK) return ret;
ret = pHub->ReadResponse(result);
if (ret != DEVICE_OK) return ret;
if (result != "ok") {
LogMessage("Expected OK");
}
return ret;
}
int RAMPSXYStage::Initialize()
{
LogMessage("XYStage: initialize");
RAMPSHub* pHub = static_cast<RAMPSHub*>(GetParentHub());
if (pHub)
{
char hubLabel[MM::MaxStrLength];
pHub->GetLabel(hubLabel);
SetParentID(hubLabel); // for backward comp.
}
else
LogMessage(NoHubError);
if (initialized_)
return DEVICE_OK;
// set property list
// -----------------
// Name
int ret = CreateStringProperty(MM::g_Keyword_Name, g_XYStageDeviceName, true);
if (DEVICE_OK != ret)
return ret;
// Description
ret = CreateStringProperty(MM::g_Keyword_Description, "RAMPS XY stage driver", true);
if (DEVICE_OK != ret)
return ret;
CPropertyAction* pAct = new CPropertyAction (this, &RAMPSXYStage::OnStepSize);
CreateProperty(g_StepSizeProp, CDeviceUtils::ConvertToString(stepSize_um_), MM::Float, false, pAct);
// Update lower and upper limits. These values are cached, so if they change during a session, the adapter will need to be re-initialized
ret = UpdateStatus();
if (ret != DEVICE_OK)
return ret;
initialized_ = true;
return DEVICE_OK;
}
int RAMPSZStage::Home() {
RAMPSHub* pHub = static_cast<RAMPSHub*>(GetParentHub());
pHub->PurgeComPortH();
int ret = pHub->SendCommand("G28 Z0");
if (ret != DEVICE_OK) {
LogMessage("Homing command failed.");
return ret;
}
std::string answer;
ret = pHub->ReadResponse(answer, 50000);
if (ret != DEVICE_OK) {
LogMessage("error getting response to homing command.");
return ret;
}
if (answer != "ok") {
LogMessage("Homing command: expected ok.");
return DEVICE_ERR;
}
return DEVICE_OK;
}
int RAMPSZStage::SetAdapterOriginUm(double z) {
RAMPSHub* pHub = static_cast<RAMPSHub*>(GetParentHub());
pHub->PurgeComPortH();
std::string xval = std::to_string((long double) z);
std::string command = "G92 Z" + xval;
int ret = pHub->SendCommand(command);
if (ret != DEVICE_OK) {
LogMessage("Origin command failed.");
return ret;
}
std::string answer;
ret = pHub->ReadResponse(answer);
if (ret != DEVICE_OK) {
LogMessage("error getting response to origin command.");
return ret;
}
if (answer != "ok") {
LogMessage("origin command: expected ok.");
return DEVICE_ERR;
}
return DEVICE_OK;
}
bool RAMPSZStage::Busy()
{
RAMPSHub* pHub = static_cast<RAMPSHub*>(GetParentHub());
return pHub->Busy();
}
