DPPanTiltControl.cpp

#include "DPPanTiltControl.hh"
#include "Base/Common/W32Compat.hh"
#include "Base/Common/BIASpragma.hh"
#include <math.h>
#include <string.h>
#include <time.h>
#include <sstream>

using namespace std;
using namespace BIAS;

DPPanTiltControl::DPPanTiltControl()
{
  COMstream_ = PORT_NOT_OPENED;
  bDefaultOrientation_ = true;
  maxSpeed_ = DPPTU_MAXSPEED;
  panResolution_ = tiltResolution_ = DPPTU_DEF_RES;
}

DPPanTiltControl::~DPPanTiltControl()
{
}

int DPPanTiltControl::SetMode(int nMode)
{
  char status;
  int ret = 0;
  set_mode(POSITION_LIMITS_MODE, ON_MODE);
  if (nMode == SPEEDMODE)
  {
    status = set_mode(SPEED_CONTROL_MODE,PTU_PURE_VELOCITY_SPEED_CONTROL_MODE);
    if (status == TRUE) {
      std::cout << "Error : Set SPEED_CONTROL_MODE failed!" << std::endl;
      ret = 1;
    } else {
      std::cout << "DPPanTiltControl : SPEED_CONTROL_MODE set" << std::endl;
    }
  }
  else if(nMode == ABSOLUTEMODE)
  {
    status = set_mode(COMMAND_EXECUTION_MODE,EXECUTE_IMMEDIATELY);
    if (status == TRUE) {
      std::cout << "Error : Set COMMAND_EXECUTION_MODE/EXECUTE IMMEDIATELY failed!" << std::endl;
      ret = 1;
    } else {
      std::cout << "DPPanTiltControl : COMMAND_EXECUTION_MODE/EXECUTE IMMEDIATELY set" << std::endl;
    }
  } else {
    std::cout << "Error : Wrong mode set for DPPanTiltControl!" << endl;
  }
  return ret;
}

bool DPPanTiltControl::ResetPanTiltUnit()
{
  int steps[2];
  steps[0] = 0;
  steps[1] = 0;
  bool bRet = false;
  switch (reset_PTU_parser(5000))
  {
    case PTU_OK:    
      SetMode(SPEEDMODE);
      set_desired(PAN,SPEED, (PTU_PARM_PTR *) &maxSpeed_, ABSOLUTE);
      set_desired(TILT,SPEED, (PTU_PARM_PTR *) &maxSpeed_, ABSOLUTE);
      await_completion();
      SetMode(ABSOLUTEMODE);
      set_desired(PAN, POSITION, (PTU_PARM_PTR *) &steps[0], ABSOLUTE);
      set_desired(TILT, POSITION, (PTU_PARM_PTR *) &steps[1], ABSOLUTE);
      await_completion();
      bRet = true;
      break;
    case PTU_FIRMWARE_VERSION_TOO_LOW:  
      bRet = false;
      return bRet;
    case PTU_NOT_RESPONDING:
      bRet = false;
      return bRet;
  }
  return bRet;
}

void DPPanTiltControl::StopPanTiltUnit()
{
  int speed = 0;
  set_desired(PAN,SPEED, (PTU_PARM_PTR *) &speed, ABSOLUTE); 
  set_desired(TILT,SPEED, (PTU_PARM_PTR *) &speed, ABSOLUTE);
}

void DPPanTiltControl::AwaitCompletion()
{
  await_completion();
}

bool DPPanTiltControl::InitPanTiltUnit(int nPort, bool bUSB)
{
  std::string portPrefix;
#ifdef WIN32
  if (bUSB)
    portPrefix = "USB";
  else
    portPrefix = "COM";
#else
  if (bUSB)
    portPrefix = "/dev/ttyUSB";
  else
    portPrefix = "/dev/ttyS";
#endif
  char portName[32];
  sprintf(portName, "%s%d", portPrefix.c_str(), nPort);
  return InitPanTiltUnit(portName);
}

bool DPPanTiltControl::InitPanTiltUnit(const char *portName)
{ 
  //reset_ptu();
  bool bRet = false;    
  std::cout << "DPPanTiltControl : Opening port " << portName << std::endl;
  biasusleep(500);
  COMstream_ = open_host_port((char*)portName);
  if (COMstream_ == PORT_NOT_OPENED) {
    return bRet;
  }
  set_mode(DEFAULTS, RESTORE_FACTORY_SETTINGS);
  switch (reset_PTU_parser(5000))
  {
    case PTU_OK:
      bRet = true;
      break;
    case PTU_FIRMWARE_VERSION_TOO_LOW:
      std::cout << "Error : PTU firmware version is too low!" << std::endl;
      bRet = false;
      break;
    case PTU_NOT_RESPONDING:
      std::cout << "Error : PTU is not responding!" << std::endl;
      bRet = false;
      break;
  }
  if (bRet) {
    char *fw = firmware_version();
    char ok = firmware_version_OK();
    bRet = (ok == TRUE);
    if (!bRet) {
      std::cout << "Error : PTU firmware version is not OK!" << std::endl;
    }
  }
  if (!bRet) {
    close_host_port(COMstream_);
  } else {
    double a,b;
    QueryResolution(a,b,true);
  }
  return bRet;
}

void DPPanTiltControl::ClosePanTiltUnit()
{
  int nPanPos = 0;
  int nTiltPos = 0;
  set_desired(PAN,POSITION,(PTU_PARM_PTR *) &nPanPos, ABSOLUTE);
  set_desired(TILT,POSITION,(PTU_PARM_PTR *) &nTiltPos, ABSOLUTE);
  set_desired(PAN,SPEED,(PTU_PARM_PTR *) &nPanPos, ABSOLUTE);
  set_desired(TILT,SPEED,(PTU_PARM_PTR *) &nTiltPos, ABSOLUTE);
  await_completion();
  close_host_port(COMstream_);
}

char DPPanTiltControl::SetSpeed(int nPanSpeed, int nTiltSpeed)
{   
  char status;
  status = set_desired(PAN,SPEED,(PTU_PARM_PTR *) &nPanSpeed, ABSOLUTE);
  status = set_desired(TILT,SPEED,(PTU_PARM_PTR *) &nTiltSpeed, ABSOLUTE);
  return status;
}

void DPPanTiltControl::GetSpeed(long &panSpeed, long &tiltSpeed)
{  
  panSpeed = get_current(PAN, SPEED);
  tiltSpeed = get_current(TILT, SPEED);
}

char DPPanTiltControl::SetAcceleration(int nPanAcc, int nTiltAcc)
{
  char status = set_desired(PAN,ACCELERATION,(PTU_PARM_PTR *) &nPanAcc, ABSOLUTE);
  if(status == PTU_OK)
    status = set_desired(TILT,ACCELERATION,(PTU_PARM_PTR *) &nTiltAcc, ABSOLUTE);
  return status;
}

void DPPanTiltControl::GetAcceleration(long &panAcc, long &tiltAcc)
{
  panAcc = get_current(PAN, ACCELERATION);
  tiltAcc = get_current(TILT, ACCELERATION);
}

void DPPanTiltControl::MoveUp(int nValue)
{ 
  if (!bDefaultOrientation_)    
    nValue = -nValue;
  set_desired(TILT,POSITION,(PTU_PARM_PTR *) &nValue, RELATIVE);
  await_completion();
}

void DPPanTiltControl::MoveDown(int nValue)
{
  if (bDefaultOrientation_)
    nValue = -nValue;
  set_desired(TILT,POSITION,(PTU_PARM_PTR *) &nValue, RELATIVE);
  await_completion();
}

void DPPanTiltControl::MoveLeft(int nValue)
{
  if (!bDefaultOrientation_)
    nValue = -nValue;
  set_desired(PAN,POSITION,(PTU_PARM_PTR *) &nValue, RELATIVE); 
  await_completion();
}
 
void DPPanTiltControl::MoveRight(int nValue)
{
  if (bDefaultOrientation_)    
    nValue = -nValue;
  set_desired(PAN,POSITION,(PTU_PARM_PTR *) &nValue, RELATIVE);
  await_completion(); 
}

void DPPanTiltControl::
GetCurrentPosition(long &lPanPos, long &lTiltPos)
{
  lPanPos = get_current(PAN,POSITION);
  lTiltPos = get_current(TILT,POSITION);
}

void DPPanTiltControl::
GetCurrentPositionDegrees(double &dPanDeg, double &dTiltDeg)
{
  dPanDeg = get_current(PAN,POSITION) * panResolution_;
  dTiltDeg = get_current(TILT,POSITION) * tiltResolution_;
}

void DPPanTiltControl::
GetCurrentPositionRadians(double &dPanRad, double &dTiltRad)
{
  dPanRad = get_current(PAN,POSITION) * panResolution_ * M_PI / 180.0;
  dTiltRad = get_current(TILT,POSITION) * tiltResolution_ * M_PI / 180.0;
}

void DPPanTiltControl::
SetPosition(int nPanPos, int nTiltPos, bool bWaitComplete)
{
  char status =  set_desired(PAN,POSITION,(PTU_PARM_PTR *) &nPanPos, ABSOLUTE);
  if (status != PTU_OK)
    std::cout << "DPPanTiltControl : Setting pan position failed!" << std::endl;

  status = set_desired(TILT,POSITION,(PTU_PARM_PTR *) &nTiltPos, ABSOLUTE);
  if (status != PTU_OK)
    std::cout << "DPPanTiltControl : Setting tilt position failed!" << std::endl;

  if (bWaitComplete)
    await_completion();
}

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

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

int DPPanTiltControl::
SetMotorStepping(int panStepping, int tiltStepping)
{
  int ret = 0;
  std::string command = "";
  string s;
  SendRawCommand("PR\n");
  ReadRawLine(s);
  ReadRawLine(s);
  switch (panStepping) {
    case AUTO:
      if (s.find("A")!=string::npos) {
        std::cout << "DPPanTiltControl : Pan stepping is already set!" << std::endl;
        panStepping = -1;
      } else {
        command="WPA\n";
      }
      break;
    case FULL:
      if (s.find("F")!=string::npos) {
        std::cout << "DPPanTiltControl : Pan stepping is already set!" << std::endl;
        panStepping = -1;
      } else {
        command="WPF\n";
      }
      break;
    case HALF:
      if (s.find("H")!=string::npos) {
        std::cout << "DPPanTiltControl : Pan stepping is already set!" << std::endl;
        panStepping = -1;
      } else {
        command="WPH\n";
      }
      break;
    case QUARTER:
      if (s.find("Q")!=string::npos) {
        std::cout << "DPPanTiltControl : Pan stepping is already set!" << std::endl;
        panStepping = -1;
      } else {
        command="WPQ\n";
      }
      break;
    case EIGHTH:
      if (s.find("E")!=string::npos) {
        std::cout << "DPPanTiltControl : Pan stepping is already set!" << std::endl;
        panStepping = -1;
      } else {
        ret = -1;
        command="WPE\n";
      }
      break;
    default:
      ret = -1;
      break;
  }
  if (ret != 0) 
    return ret;

  if (panStepping != -1)
  {
    SendRawCommand(command);
    // TODO When switching stepping mode, pan tilt recalibrates.
    //      "Await completion" does not work here. Wait until finished.
    while (true) {
      if (ReadRawLine(s) != 0) {
        std::cout << "DPPanTiltControl : Read " << s << std::endl;
        if (s.find("*") != string::npos) {
          break;
        }
      }
      std::cout << "DPPanTiltControl : Sleeping for 1 second" << std::endl;
      biassleep(1);
    }
  }

  SendRawCommand("TR\n");
  ReadRawLine(s);
  ReadRawLine(s);
  switch (tiltStepping) {
    case AUTO:
      if (s.find("A")!=string::npos) {
        std::cout << "DPPanTiltControl : Tilt stepping is already set!" << std::endl;
        tiltStepping = -1;
      } else {
        command="WTA\n";
      }
      break;
    case FULL:
      if (s.find("F")!=string::npos) {
        std::cout << "DPPanTiltControl : Pan stepping is already set!" << std::endl;
        tiltStepping = -1;
      } else {
        command="WTF\n";
      }
      break;
    case HALF:
      if (s.find("H")!=string::npos) {
        std::cout << "DPPanTiltControl : Pan stepping is already set!" << std::endl;
        tiltStepping = -1;
      } else {
        command="WTH\n";
      }
      break;
    case QUARTER:
      if (s.find("Q")!=string::npos) {
        std::cout << "DPPanTiltControl : Pan stepping is already set!" << std::endl;
        tiltStepping = -1;
      } else {
        command="WTQ\n";
      }
      break;
    case EIGHTH:
      if (s.find("E")!=string::npos) {
        std::cout << "DPPanTiltControl : Pan stepping is already set!" << std::endl;
        tiltStepping = -1;
      } else {
        command="WTE\n";
      }
      break;
    default:
      ret = -1;
      break;

  }
  if (ret != 0) 
    return ret;

  if (tiltStepping != -1)
  {
    SendRawCommand(command);
    // TODO See above!
    while (true)
    {
      if (ReadRawLine(s) != 0) {
        std::cout << "DPPanTiltControl : Read " << s << std::endl;
        if (s.find("*") != string::npos) {
          break;
        }
      }
      std::cout << "DPPanTiltControl : Sleeping for 1 second" << std::endl;
      biassleep(1);
    }
  }

  double a, b;
  std::cout << "DPPanTiltControl : Sleeping for 1 second" << std::endl;
  biassleep(1);
  QueryResolution(a, b, true);
  return ret;
}

void DPPanTiltControl::
QueryResolution(double &panResolution, double &tiltResolution, bool updateInternal)
{
  if (updateInternal) {
    string s, tmp;
    double argsec;
    SendRawCommand("PR\n");
    ReadRawLine(s); // this is the command
    ReadRawLine(s); // this is the answer
    stringstream ss1;
    ss1 << s;
    ss1 >> tmp;
    ss1 >> argsec;
    //std::cout << argsec << std::endl;
    panResolution_ = argsec / 3600.0;
    SendRawCommand("TR\n");
    ReadRawLine(s); // this is the command
    ReadRawLine(s); // this is the answer
    stringstream ss2;
    ss2 << s;
    ss2 >> tmp;
    ss2 >> argsec;
    //std::cout << argsec << std::endl;
    tiltResolution_ = argsec / 3600.0;
  }
  panResolution = panResolution_;
  tiltResolution = tiltResolution_;
}