IselLinearControl.cpp

#include "IselLinearControl.hh"
#include <Base/Common/W32Compat.hh>
#include <Base/Common/BIASpragma.hh>
#include <Base/Debug/Error.hh>
#include <cstring>
#include <sstream>

using namespace std;
using namespace BIAS;

IselLinearControl::IselLinearControl()
{
  speed_ = 40000;
  steps_per_mm_ = 640; // constant for our isel control
}

IselLinearControl::~IselLinearControl() {}


int IselLinearControl::Reset()
{
  SendRawCommand("@0R1\r");
  return CheckStatus();
}

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

int IselLinearControl::Init(const char *COMportName)
{ 
  cout << "YoYoYo" << endl;
  cout << "Opening port " << COMportName << " for Isel Linear."<<endl;
  COMstream_ = open_host_port((char*)COMportName);
  if (COMstream_ == PORT_NOT_OPENED) { 
    return -1;
  }
  SendRawCommand("@01\r");
  return CheckStatus();
}

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


int IselLinearControl::SetSpeed(int speed)
{
  if (speed > 0 && speed <= 40000) {
    speed_ = speed;
    return 0;
  } else {
    BIASERR("speed out of bounds, use value 0..40000");
    return -1;
  }
}

void IselLinearControl::GetSpeed(int &speed)
{
  speed = speed_;
}

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

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

int IselLinearControl::MoveLeft(float millimeters)
{
  float steps = millimeters * steps_per_mm_;
  int nValue = (int)steps;
  if (steps - (float)nValue > 0.0f) {
    BIASWARN("\nwant to move " << steps << " steps. flooring to " << nValue << " steps, loosing precision.\n"
        <<   "Isel control does 1/"<<steps_per_mm_<<" steps.");
  }
  stringstream s;
  s << "@0A" << nValue << "," << speed_ << "\r";
  SendRawCommand(s.str());
  return CheckStatus();
}
 
int IselLinearControl::MoveRight(float millimeters)
{
  float steps = millimeters * steps_per_mm_;
  int nValue = (int)steps;
  if (steps - (float)nValue > 0.0f) {
    BIASWARN("\nwant to move " << steps << " steps. flooring to " << nValue << " steps, loosing precision.\n"
        <<   "Isel control does 1/"<<steps_per_mm_<<" steps.");
  }
  nValue = -nValue;
  stringstream s;
  s << "@0A" << nValue << "," << speed_ << "\r";
  SendRawCommand(s.str());
  return CheckStatus();
}

void IselLinearControl::GetCurrentPosition(int &pos)
{
  // TODO
  BIASERR("implement me");
}

int IselLinearControl::SetPosition(int pos, bool bWaitComplete)
{
  stringstream s;
  s << "@0M" << pos << "," << speed_ << "\r";
  SendRawCommand(s.str());
  if (bWaitComplete) {
    return CheckStatus();
  }
  return 0;
}

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

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

int IselLinearControl::CheckStatus() {
  string test;
  while (ReadRawLine(test) == 0) {}
  if (test.length() == 1) {
    unsigned char c = test.c_str()[0];
    switch (c) {
      case '0':
        // no error
        return 0;
      case '2':
        BIASERR("reached limit, need to move free");
        SendRawCommand("@0F1\r");
        return CheckStatus();
      default:
        BIASERR("Got Errorcode: " << test);
        return -1;
    }
  }
  BIASERR("unknown response: " << test);
  return -1;
}