IselLinearControlTwoAxis.cpp

#include "IselLinearControlTwoAxis.hh"
#include <Base/Common/W32Compat.hh>
#include <Base/Common/BIASpragma.hh>
#include <Base/Debug/Error.hh>
#include <cstring>
#include <sstream>
#ifndef WIN32
#  include <termios.h>
#endif

using namespace std;
using namespace BIAS;

IselLinearControlTwoAxis::IselLinearControlTwoAxis()
{
  steps_per_second_x_ = 40000;
  steps_per_second_y_ = 40000;
  //preset defaults, can be overwritten by Init.
  steps_per_mm_x_ = 160;
  steps_per_mm_y_ = 320;
}

IselLinearControlTwoAxis::IselLinearControlTwoAxis(const int steps_per_mm_x, const int steps_per_mm_y)
{
  steps_per_mm_x_ = steps_per_mm_x;
  steps_per_mm_y_ = steps_per_mm_y;
}

IselLinearControlTwoAxis::~IselLinearControlTwoAxis() {}

int IselLinearControlTwoAxis::Reset(const int axis)
{
  stringstream cmd;
  cmd << "@0R" << axis << "\r";

  // axis should be 1-x, 2-y, or 3 x+y
  if((axis > 0)&& (axis<4)){
    SendRawCommand(cmd.str());
    return CheckStatus();
  } else {
    return -1;
  }
}

int IselLinearControlTwoAxis::Init(const int nCOMportNumber)
{
  char COMportPrefix[10] = "/dev/ttyS";
  char COMportName[17];
  sprintf(COMportName, "%s%d", COMportPrefix, nCOMportNumber);
  return Init(COMportName);
}

int IselLinearControlTwoAxis::Init(const char *COMportName)
{ 
  cout << "Opening port " << COMportName << " for Isel Linear."<<endl;
  COMstream_ = open_host_port((char*)COMportName);
  if (COMstream_ == PORT_NOT_OPENED) {
    return -1;
  }

#ifndef WIN32
  // ofl: Set baud rate to 19200
  struct termios info;

  info.c_iflag = IGNBRK | IGNPAR;
  info.c_iflag &= ~(INLCR | ICRNL | IUCLC | ISTRIP | IXON | BRKINT);
  info.c_oflag &= ~OPOST;
  info.c_lflag &= ~(ICANON | ISIG | ECHO);
  info.c_cflag = B19200 | CS8 | CREAD | CLOCAL;

  if (tcsetattr(COMstream_, TCSANOW, &info) < 0)
  {
     cout << "Error using TCSETS in ioctl" << endl;
     close_host_port(COMstream_);
     return -1;
  }
#endif
 
  SendRawCommand("@03\r");
  return CheckStatus();
}

void IselLinearControlTwoAxis::Close()
{
  close_host_port(COMstream_);
}


int IselLinearControlTwoAxis::SetSpeed(const float speedX, const float speedY)
{
  int tmp_speed_in_steps_x = (int) speedX * steps_per_mm_x_;
  int tmp_speed_in_steps_y = (int) speedY * steps_per_mm_y_;

  if (tmp_speed_in_steps_x > 0 && tmp_speed_in_steps_x <= 40000 &&
      tmp_speed_in_steps_y > 0 && tmp_speed_in_steps_y <= 40000) {
    steps_per_second_x_ = tmp_speed_in_steps_x;
    steps_per_second_y_ = tmp_speed_in_steps_y;
    return 0;
  } else {
    stringstream tmp;
    tmp << "speed out of bounds, use value 1..40000 steps per second (" << (1/steps_per_mm_x_) << " < x < "<< (40000/steps_per_mm_x_) <<", " << (1/steps_per_mm_y_) << " < y < "<< (40000/steps_per_mm_y_) <<" [mm/s])";
    BIASERR(tmp.str());
    return -1;
  }
}

void IselLinearControlTwoAxis::GetSpeed(float &speedX, float &speedY)
{
  speedX = steps_per_second_x_ / steps_per_mm_x_;
  speedY = steps_per_second_y_ / steps_per_mm_y_;
}

int IselLinearControlTwoAxis::SetAcceleration(const int accel)
{
  /* TODO
  stringstream s;
  s << "@0_" << accel << "\r";
  SendRawCommand(s.str());
  return CheckStatus();
  */
  BIASERR("implement me");
  return 0;
}

void IselLinearControlTwoAxis::GetAcceleration(int &accel)
{
  // TODO
  BIASERR("implement me");
}

int IselLinearControlTwoAxis::Move(const float millimetersX, const float millimetersY)
{
  float stepsX, stepsY;
  stepsX = millimetersX * steps_per_mm_x_;
  stepsY = millimetersY * steps_per_mm_y_;

  int nValueX = (int)stepsX;
  int nValueY = (int)stepsY;

  if (stepsX - (float)nValueX > 0.0f) {
    BIASWARN("\nwant to move " << stepsX << " steps. flooring to " << nValueX << " steps, loosing precision.\n");
  }

  if (stepsY - (float)nValueY > 0.0f) {
    BIASWARN("\nwant to move " << stepsY << " steps. flooring to " << nValueY << " steps, loosing precision.\n");
  }

  stringstream s;
  s << "@0A" << nValueX << "," << steps_per_second_x_ << "," << nValueY << "," << steps_per_second_y_ << "\r";
  SendRawCommand(s.str());
  return CheckStatus();
}

void IselLinearControlTwoAxis::GetCurrentPosition(float &millimetersX, float &millimetersY)
{
  string output;
  stringstream ss;
  int i, size = 0;
  SendRawCommand("@0P\r");
  while (size == 0) {
    size = ReadRawLine(output);
  }

  if(size >= 18){
    if(output.c_str()[size-18] >= '8')
      ss << "FF";
    else
      ss << "0";
    ss << output.substr(size-18,6);
    ss >> hex >> i;
    millimetersX = (float) i / (float) steps_per_mm_x_;

    ss.str("");
    if(output.c_str()[size-12] >= '8')
      ss << "FF";
    else
      ss << "0";
    ss << output.substr(size-12,6);
    ss >> hex >> i;
    millimetersY = (float) i / (float) steps_per_mm_y_;
  } else {
    BIASWARN("\ncant determine position! \n");
  }
}

int IselLinearControlTwoAxis::SetPosition(const float millimetersX, const float millimetersY, const bool bWaitComplete)
{
  float posX = millimetersX * (float) steps_per_mm_x_;
  float posY = millimetersY * (float) steps_per_mm_y_;

  int nValueX = (int)posX;
  int nValueY = (int)posY;

  if (posX - (float)nValueX > 0.0f) {
    BIASWARN("\nwant to move to position " << posX << " in steps on x-axis. flooring to position " << nValueX << " in steps, loosing precision.\n");
  }

  if (posY - (float)nValueY > 0.0f) {
    BIASWARN("\nwant to move to position " << posY << " in steps on y-axis. flooring to position " << nValueY << " in steps, loosing precision.\n");
  }

  stringstream s;
  s << "@0M" << nValueX << "," << steps_per_second_x_ << "," << nValueY << "," << steps_per_second_y_ << "\r";
  SendRawCommand(s.str());

  if (bWaitComplete) {
    return CheckStatus();
  }
  return 0;
}

void IselLinearControlTwoAxis::SendRawCommand(const string cmd) {
  SerialStringOut(COMstream_, (unsigned char*) cmd.c_str());

}

int IselLinearControlTwoAxis::ReadRawLine(string &cmd) {
    char result[0xFF];
    int len = 0;
    ReadSerialLine(COMstream_, (unsigned char*)result,1,&len);
    cmd = string(result,len);
    return len;
}

int IselLinearControlTwoAxis::CheckStatus() {
  string test;
  unsigned int i = 0;
  while (ReadRawLine(test) == 0) {
  }
  while (i < test.size() && test.c_str()[i] == '0') {
    i++;
  }
  if (test.size() == i) {
    // no error
    return 0;
  } else {
    unsigned char c = test.c_str()[i];
    switch (c) {
    case '2':
      BIASERR("reached limit - move free and reset");
      SendRawCommand("@0F3\r");
      return (CheckStatus() == 0 ? Reset() : -1);
    case '9':
      BIASERR("possible emergency stop - reinit and reset");
      SendRawCommand("@03\r");
      return (CheckStatus() == 0 ? Reset() : -1);
    case 'D':
      BIASERR("speed out of bounds");
      return -1;
    case 'R':
      BIASERR("need to reset");
      return Reset();
    default:
      BIASERR("Got Errorcode: " << test);
      return -1;
    }
  }
  BIASERR("unknown response: " << test);
  return -1;
}