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;
}
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 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;
}
