view/runCtrl/RunCtrl.cpp

/*!
  \file
  \brief s§äCu

  \author Satofumi KAMIMURA

  $Id$
*/

#include "RunCtrl.h"
#include "SerialCtrl.h"
#include "TransmitCtrl.h"
#include "CalculateGeometry.h"
#include "structTable.h"
#include "tRunCtrl.h"
#include "FileToArgs.h"
#include "SearchFilePath.h"
#include <cstring>
#include <cstdlib>

using namespace beego;


struct RunCtrl::pImpl {
  enum {
    Timeout = 100,
    RetryTimes = 3,
  };
  std::string error_message;
  ConnectionInterface* con;

  typedef std::vector<unsigned char> CommandMessage;
  CommandMessage last_command;
  TransmitCtrl* transmit;
  tRunCtrl_t top;

  /*!
    \brief RunCtrl ĚřÇ
  */
  class ArgsInfo {
  public:
    std::string port;
    long baudrate;

    ArgsInfo(void) : baudrate(DefaultBaudrate) {
    }
  };

  pImpl(void)
    : error_message("no error."), con(new SerialCtrl), transmit(NULL) {
  }

  ~pImpl(void) {
    delete transmit;
    delete con;
  }

  void parseArgs(ArgsInfo& args_info, int argc, char *argv[]) {

    for (int i = 1; i < argc; ++i) {
      if (! strncmp("--run_port=", argv[i], 11)) {
        args_info.port = &argv[i][11];
      } else if (! strncmp("--run_baudrate=", argv[i], 15)) {
        args_info.baudrate = atoi(&argv[i][15]);
      }
    }
  }

  bool connect(ConnectionInterface* con) {

    transmit = new TransmitCtrl(con, reinterpret_cast<unsigned char*>(&top));
    // !!! ăLĚśŹÉ¸sľ˝çAOL^đs¤

    // o[W`FbN
    // !!!
    // !!! ŞČŻęÎAdšŞüÁÄ˘éŠH ĆĚG[bZ[W
    // !!! o[WŞŮČęÎAÄŤźľÄËAĆĚG[bZ[W
    // !!!
    // !!! RunIdHandler ŠH

    // ĘMe[uĚöItZbgĚmF
    int target_lastOffset = getTargetLastOffset();
    int host_lastOffset = getHostLastOffset();
    if (target_lastOffset != host_lastOffset) {
      error_message = "struct tables is mismatch between host and target.";
      return false;
    }

    // ŔWnĘuĚúť
    // !!!

    // Watch Dog Timer Ěúť
    // !!!

    // ^CX^vĚúť
    // !!!

    return true;
  }

  bool isConnected(void) {
    return con->isConnected();
  }

  bool isNotConnected(void) {
    if (! isConnected()) {
      error_message = "not connected.";
      return true;
    } else {
      return false;
    }
  }

  int getTargetLastOffset(void) {
    transmit->initPacket(READ_PACKET);
    ADD_READ_VARIABLE((*transmit), top.last_offset);
    transmit->sendWithRetry(last_command, RetryTimes, Timeout);

    return top.last_offset;
  }

  int getHostLastOffset(void) {
    return (reinterpret_cast<unsigned char*>(&top.last_offset)
            - reinterpret_cast<unsigned char*>(&top));
  }

  int to_div16(const Angle& angle) {
    return static_cast<int>(65536.0 * angle.to_rad() / (2.0 * M_PI));
  }

  void setStraightHoldVelocity(StraightCtrlMode mode,
                               const int velocity, const int acceleration) {

    ADD_WRITE_VARIABLE((*transmit), top.mode.straight.mode, mode);
    ADD_WRITE_VARIABLE((*transmit), top.mode.straight.vel.ref_vel, velocity);
    ADD_WRITE_VARIABLE((*transmit), top.mode.straight.vel.acc, acceleration);
    ADD_WRITE_VARIABLE((*transmit), top.mode.straight.vel.is_stable, FALSE);
  }

  void setRotateCtrl(RotateCtrlMode mode,
                     const Angle& velocity, const Angle& acceleration) {

    ADD_WRITE_VARIABLE((*transmit), top.mode.rotate.mode, mode);
    ADD_WRITE_VARIABLE((*transmit),
                       top.mode.rotate.vel.ref_vel, to_div16(velocity));
    ADD_WRITE_VARIABLE((*transmit),
                       top.mode.rotate.vel.acc, to_div16(acceleration));
    ADD_WRITE_VARIABLE((*transmit), top.mode.rotate.vel.is_stable, FALSE);
  }

  void stop(void) {
    transmit->initPacket(WRITE_PACKET);
    ADD_WRITE_VARIABLE((*transmit), top.mode.ctrl, NORMAL_CONTROL);

    // !!! {ÍAp[^ŠçÁŹxđóŻćé
    int acceleration_mm = 600;
    setStraightHoldVelocity(STRAIGHT_HOLD_VELOCITY_FIRST, 0, acceleration_mm);

    // !!! {ÍAp[^ŠçÁŹxđóŻćé
    Angle acceleration_angle = deg(90);
    setRotateCtrl(ROTATE_HOLD_VELOCITY_FIRST, deg(0), acceleration_angle);

    transmit->sendWithRetry(last_command, RetryTimes, Timeout);
    // !!! óMÉ¸sľ˝çA˝Ě˝ßđžÁ˝Šđ\ŚľÄáOđ°é
  }

  void set_servoFree(void) {
    transmit->initPacket(WRITE_PACKET);
    ADD_WRITE_VARIABLE((*transmit), top.mode.ctrl, DIRECT_SERVO_FREE);
    ADD_WRITE_VARIABLE((*transmit), top.mode.straight.vel.is_stable, TRUE);
    ADD_WRITE_VARIABLE((*transmit), top.mode.rotate.vel.is_stable, TRUE);

    transmit->sendWithRetry(last_command, RetryTimes, Timeout);
    // !!! óMÉ¸sľ˝çA˝Ě˝ßđžÁ˝Šđ\ŚľÄáOđ°é
  }

  size_t getRawTimestamp(void) {
    transmit->initPacket(READ_PACKET);
    ADD_READ_VARIABLE((*transmit), top.sec);
    ADD_READ_VARIABLE((*transmit), top.msec);

    transmit->sendWithRetry(last_command, RetryTimes, Timeout);
    // !!! óMÉ¸sľ˝çA˝Ě˝ßđžÁ˝Šđ\ŚľÄáOđ°é

    return (top.sec * 1000) + top.msec;
  }

  int getMmLength(const gridPosition_t& position, int index) {

    int mm_length = position.mm[index] +
      (1000 * (position.m[index] + (1000 * position.km[index])));

    return mm_length;
  }

  void initIsStable(void) {
    ADD_WRITE_VARIABLE((*transmit), top.mode.straight.vel.is_stable, FALSE);
    ADD_WRITE_VARIABLE((*transmit), top.mode.rotate.vel.is_stable, FALSE);
  }

  bool isStable(void) {
    transmit->initPacket(READ_PACKET);
    // !!! sec, msec ŕóMˇé
    ADD_READ_VARIABLE((*transmit), top.mode.straight.vel.is_stable);
    ADD_READ_VARIABLE((*transmit), top.mode.rotate.vel.is_stable);

    transmit->sendWithRetry(last_command, RetryTimes, Timeout);
    // !!! óMÉ¸sľ˝çA˝Ě˝ßđžÁ˝Šđ\ŚľÄáOđ°é

#if 0
    fprintf(stderr, "%d, %d\n",
            top.mode.straight.vel.is_stable,
            top.mode.rotate.vel.is_stable);
#endif

    return ((top.mode.straight.vel.is_stable == TRUE) &&
            (top.mode.rotate.vel.is_stable == TRUE)) ? true : false;
  }

  void getPosition(Position<int>& position) {
    transmit->initPacket(READ_PACKET);
    // !!! sec, msec ŕóMˇé
    for (int i = 0; i < 2; ++i) {
      ADD_READ_VARIABLE((*transmit), top.bodyPosition.robot.km[i]);
      ADD_READ_VARIABLE((*transmit), top.bodyPosition.robot.m[i]);
      ADD_READ_VARIABLE((*transmit), top.bodyPosition.robot.mm[i]);
      ADD_READ_VARIABLE((*transmit), top.bodyPosition.cnt_integer[i]);
      ADD_READ_VARIABLE((*transmit), top.bodyPosition.cnt_decimal[i]);
    }
    ADD_READ_VARIABLE((*transmit), top.bodyPosition.robot.div16);
    ADD_READ_VARIABLE((*transmit), top.bodyPosition.straight_vel);

    transmit->sendWithRetry(last_command, RetryTimes, Timeout);
    // !!! óMÉ¸sľ˝çA˝Ě˝ßđžÁ˝Šđ\ŚľÄáOđ°é

    position.x = getMmLength(top.bodyPosition.robot, X);
    position.y = getMmLength(top.bodyPosition.robot, Y);
    position.angle = rad(2.0 * M_PI * top.bodyPosition.robot.div16 / 65536);
  }

  int getVelocity_straight(void) {
    transmit->initPacket(READ_PACKET);
    ADD_READ_VARIABLE((*transmit), top.straight_vel.averaged_vel);

    transmit->sendWithRetry(last_command, RetryTimes, Timeout);
    // !!! óMÉ¸sľ˝çA˝Ě˝ßđžÁ˝Šđ\ŚľÄáOđ°é

    return top.straight_vel.averaged_vel;
  }

  void direct_setWheelVelocity(const int id, int velocity_mm) {
    transmit->initPacket(WRITE_PACKET);
    ADD_WRITE_VARIABLE((*transmit), top.mode.ctrl, DIRECT_WHEEL_CONTROL);
    ADD_WRITE_VARIABLE((*transmit),
                       top.direct[id].whl_target_mm_sec, velocity_mm);
    transmit->sendWithRetry(last_command, RetryTimes, Timeout);
    // !!! óMÉ¸sľ˝çA˝Ě˝ßđžÁ˝Šđ\ŚľÄáOđ°é
  }

  void direct_setStraightVelocity(const int velocity_mm) {
    transmit->initPacket(WRITE_PACKET);
    ADD_WRITE_VARIABLE((*transmit), top.mode.ctrl, NORMAL_CONTROL);
    // !!! {ÍAp[^ŠçÁŹxđóŻćé
    int acceleration_mm = 600;
    setStraightHoldVelocity(STRAIGHT_HOLD_VELOCITY_FIRST,
                            velocity_mm, acceleration_mm);

    transmit->sendWithRetry(last_command, RetryTimes, Timeout);
    // !!! óMÉ¸sľ˝çA˝Ě˝ßđžÁ˝Šđ\ŚľÄáOđ°é
  }

  void direct_setRotateVelocity(const Angle& velocity) {
    transmit->initPacket(WRITE_PACKET);
    ADD_WRITE_VARIABLE((*transmit), top.mode.ctrl, NORMAL_CONTROL);
    // !!! {ÍAp[^ŠçÁŹxđóŻćé
    Angle acceleration = deg(90);
    setRotateCtrl(ROTATE_HOLD_VELOCITY_FIRST, velocity, acceleration);

    transmit->sendWithRetry(last_command, RetryTimes, Timeout);
    // !!! óMÉ¸sľ˝çA˝Ě˝ßđžÁ˝Šđ\ŚľÄáOđ°é
  }

  void stop_toAngle(const Angle& target) {

    // !!! ąąÖĚ target ÍAÜARunCtrl ĚŔWČĚŠH
    // !!! ŔWaÜíčĚhLgÉÄč`ˇ×Ť

    transmit->initPacket(WRITE_PACKET);
    ADD_WRITE_VARIABLE((*transmit), top.mode.ctrl, NORMAL_CONTROL);

    // !!! ŔiÉÖˇép[^§äÜíčŕ˛Žˇé

    // !!! {ÍAp[^ŠçÁŹxđóŻćé
    Angle acceleration = deg(90);
    // !!! {ÍAp[^ŠçÚWŹxđóŻćé
    Angle velocity = deg(180);
    setRotateCtrl(ROTATE_HOLD_POSITION_FIRST, velocity, acceleration);

    // !!! ÎĘuđŤŢ
    // !!! {ÍAKŘÉÝčˇ×Ť
    // !!! X-Y ŕ
    // !!! {ÍAAngle É operator - đč`ˇ×ŤžŞAŰŻ
    ADD_WRITE_VARIABLE((*transmit), top.commandBase_offset.div16,
                       -to_div16(target));
    // !!! ÂŠApxwčĚŔWnđÇ¤ˇéŠÍAľ§Éč`ˇ×Ť

    initIsStable();

    transmit->sendWithRetry(last_command, RetryTimes, Timeout);
    // !!! óMÉ¸sľ˝çA˝Ě˝ßđžÁ˝Šđ\ŚľÄáOđ°é
  }
};


RunCtrl::RunCtrl(void) : pimpl(new pImpl) {
}


RunCtrl::~RunCtrl(void) {
}


const char* RunCtrl::what(void) {
  return pimpl->error_message.c_str();
}


bool RunCtrl::connect(int argc, char *argv[]) {

  // ftHgÝčĚÇÝoľ
  std::vector<std::string> search_path;
  search_path.push_back("./");
  search_path.push_back("~/.beego/");

  pImpl::ArgsInfo args_info;
  std::string config_file;
  // !!! beegoconf ĚźOđĎŚçęéć¤ÉľÄ¨×ŤŠH ťĚ¤ż...
  if (searchFilePath(config_file, "beegoconf", search_path)) {
    // Ýčt@CĚŕeđřĆľÄľ¤
    FileToArgs fileArgs;
    fileArgs.load(config_file.c_str(), argv[0]);
    pimpl->parseArgs(args_info, fileArgs.argc, fileArgs.argv);
  }
  pimpl->parseArgs(args_info, argc, argv);

  return connect(args_info.port.c_str(), args_info.baudrate);
}


bool RunCtrl::connect(const char* device, long baudrate) {

  pimpl->con->disconnect();
  bool ret = pimpl->con->connect(device, baudrate);
  if (ret == false) {
    pimpl->error_message = pimpl->con->what();
    return false;
  }
  return connect(pimpl->con);
}


bool RunCtrl::connect(ConnectionInterface* con) {

  bool ret = pimpl->connect(con);
  if (! ret) {
    return false;
  }

  // T[{đŠŻé
  stop();
  return true;
}


void RunCtrl::disconnect(void) {

  pimpl->con->disconnect();

  delete pimpl->transmit;
  pimpl->transmit = NULL;

  pimpl->error_message = "disconnected.";
}


bool RunCtrl::isConnected(void) {
  return pimpl->isConnected();
}


ConnectionInterface*& RunCtrl::getConnection(void) {
  return pimpl->con;
}


void RunCtrl::push_runState(void) {
  // !!!
}


void RunCtrl::pop_runState(void) {
  // !!!
}


void RunCtrl::set_sendRetryTimes(size_t times) {
  // !!!
}


void RunCtrl::set_recvTimeout(size_t msec) {
  // !!!
}


void RunCtrl::set_watchDogTimer(size_t msec) {
  // !!!
}


void RunCtrl::stop(void) {
  if (pimpl->isNotConnected()) {
    return;
  }
  pimpl->stop();
}


void RunCtrl::set_servoFree(void) {
  if (pimpl->isNotConnected()) {
    return;
  }
  pimpl->set_servoFree();
}


void RunCtrl::follow_line(const CoordinateCtrl* crd,
                          const Position<int>& position) {
  if (pimpl->isNotConnected()) {
    return;
  }

  // ŔiÍŹx§ä
  // !!!

  // ń]ÍźüÇ]
  // !!!
}


void RunCtrl::follow_circle(const CoordinateCtrl* crd,
                            const Grid<int>& point, const int radius) {
  if (pimpl->isNotConnected()) {
    return;
  }

  // ŔiÍŹx§ä
  // !!!

  // ń]Í~ĘÇ]
  // !!!
}


void RunCtrl::stop_toLine(const CoordinateCtrl* crd,
                          const Position<int>& position) {
  if (pimpl->isNotConnected()) {
    return;
  }

  // ŔiÍĘu§ä
  // !!!

  // ń]ÍĘu§ä
  // !!!
}


void RunCtrl::stop_toPoint(const CoordinateCtrl* crd,
                           const Grid<int>& point) {
  if (pimpl->isNotConnected()) {
    return;
  }

  // ŔiÍĘu§ä
  // !!!

  // ń]ÍĘu§ä
  // !!!
}


void RunCtrl::move_length(const int length) {
  if (pimpl->isNotConnected()) {
    return;
  }

  // ŔiÍĘu§ä
  // !!!

  // ń]ÍĘu§ä
  // !!!
}


void RunCtrl::rotate_angle(const Angle& angle) {
  if (pimpl->isNotConnected()) {
    return;
  }

  // ŔiÍ§äČľ
  // !!!

  // ń]ÍĘu§ä
  // !!!
}


void RunCtrl::stop_toAngle(const CoordinateCtrl* crd, const Angle& angle) {
  if (pimpl->isNotConnected()) {
    return;
  }

  // ŔiÍ§äČľ
  // !!!

  // ń]ÍĘu§ä
  // !!!
  pimpl->stop_toAngle(angle);
}


bool RunCtrl::isStable(void) {
  if (pimpl->isNotConnected()) {
    return false;
  }
  return pimpl->isStable();
}


int RunCtrl::getLength_toLine(const CoordinateCtrl* crd,
                               const Position<int>& position) {
  if (pimpl->isNotConnected()) {
    return -1;
  }

  Position<int> own_pos = getPosition(crd);
  return calculate_getLength_toPosition(own_pos, position);
}


int RunCtrl::getLength_toPoint(const CoordinateCtrl* crd,
                                const Grid<int>& point) {
  if (pimpl->isNotConnected()) {
    return -1;
  }

  Position<int> own_pos = getPosition(crd);
  return calculate_getLength_toPoint(own_pos, point);
}


Angle RunCtrl::getAngle_toDirection(void) {
  if (pimpl->isNotConnected()) {
    // !!! áOđ°é×ŤŠH °˝Č˘Ş...
    return Angle(deg(-1));
  }

  // !!!
  return Angle();
}


Angle RunCtrl::getAngle_toPoint(void) {
  if (pimpl->isNotConnected()) {
    return Angle(deg(-1));
  }

  // !!!
  return Angle();
}


Angle RunCtrl::getAngle_toLine(void) {
  if (pimpl->isNotConnected()) {
    return Angle(deg(-1));
  }

  // !!!
  return Angle();
}


int RunCtrl::getVelocityDiff_straight(const int velocity) {
  if (pimpl->isNotConnected()) {
    return -1;
  }

  // !!!
  return 0;
}


Angle RunCtrl::getVelocityDiff_rotate(const Angle velocity) {
  if (pimpl->isNotConnected()) {
    return Angle(deg(-1));
  }

  // !!!
  return Angle();
}


int RunCtrl::getVelocity_straight(void) {
  if (pimpl->isNotConnected()) {
    return -1;
  }

  return pimpl->getVelocity_straight();
}


Angle RunCtrl::getVelocity_rotate(void) {
  if (pimpl->isNotConnected()) {
    return Angle(deg(-1));
  }

  // !!!
  return Angle();
}


// void GetParams::RunCtrl::*
// void SetParams::RunCtrl::*

Position<int> RunCtrl::getPosition(const CoordinateCtrl* crd) {
  if (pimpl->isNotConnected()) {
    // !!! áOđ°é×ŤŠH °˝Č˘Ş...
    return Position<int>(-1, -1, deg(-1));
  }

  Position<int> position;
  pimpl->getPosition(position);

  // !!! crd đlśľ˝ĘuđÔˇć¤ÉCł
  // !!!
  return position;
}


size_t RunCtrl::getRawTimestamp(void) {
  return pimpl->getRawTimestamp();
}


void RunCtrl::direct_setWheelVelocity(const int id, const int velocity_mm) {
  if (pimpl->isNotConnected()) {
    return;
  }
  pimpl->direct_setWheelVelocity(id, velocity_mm);
}


void RunCtrl::direct_setStraightVelocity(const int velocity_mm) {
  if (pimpl->isNotConnected()) {
    return;
  }
  pimpl->direct_setStraightVelocity(velocity_mm);
  // !!!
}


void RunCtrl::direct_setRotateVelocity(const Angle& velocity_angle) {
  if (pimpl->isNotConnected()) {
    return;
  }
  pimpl->direct_setRotateVelocity(velocity_angle);
}