tRunCtrlSimulator.cpp

/*
  tRunCtrl のシミュレートクラス
  Satofumi KAMIMURA
  $Id$
*/

#include "tRunCtrlSimulator.h"
#include "runCtrl.h"
#include "tRunCtrl.h"
#include "taskCtrl.h"
#include "transferCtrl.h"
#include "sh7045lib.h"
extern void init_sci(int port, ConnectionDevice* con);


tRunCtrl_Simulator::tRunCtrl_Simulator(void) : server(NULL), con(NULL) {
}


tRunCtrl_Simulator::~tRunCtrl_Simulator(void) {
  stop_sci(SciPort);
  delete con;
  delete server;
}


void tRunCtrl_Simulator::init(void) {
  // サーバの起動
  server = new TcpipServer();
  server->activate(RunCtrl::SimulatorPort);

  initTransferCtrl(SciPort);
  initNodeInfo(&node);
  init_tRunCtrlState(&tbl, PACKAGE_NUM_VERSION);
  registerStructInfo(&node, (unsigned char *)&tbl, RUN_CTRL_TARGET_ID);
}


void tRunCtrl_Simulator::recv(void) {
  // 接続
  if (!con) {
    con = server->accept(0);
    if (con) {
      init_sci(SciPort, con);
    }
#if 0
    TcpipDevice* connection = server->accept(0);
    if (connection) {
      con = new DelaiedTcpipDevice(connection);
      init_sci(SciPort, con);
    }
#endif
  }

  // データの受信、応答
  if (con) {
    if (con->isConnected()) {
      recv_tRunCtrlPacket(&tbl);
    } else {
      delete con;
      con = NULL;
    }
  }
}


void tRunCtrl_Simulator::exec1msec(unsigned long total_msec) {
  // 制御処理
  if (con) {
    update_tRunCtrlState(&tbl);

    VXV::Position3D sim_pos, dummy;
    sim_pos = getBodyPosition(dummy);
    ticksPos.add(sim_pos, total_msec);
  }
}


bool tRunCtrl_Simulator::updatePosition(void) {
  return true;
}


void tRunCtrl_Simulator::setLocalPosition(const VXV::Position3D& position) {

  // X, Y の更新
  for (int i = 0; i < 2; ++i) {
    tbl.bodyPos.cnt_decimal[i] = 0;
  }

  long mm[2] = { position.x, position.y };
  for (int i = 0; i < 2; ++i) {
    tbl.bodyPos.body_crd.km[i] = 0;
    tbl.bodyPos.body_crd.m[i] = 0;
    tbl.bodyPos.body_crd.mm[i] = 0;

    while (mm[i] >= 1000000) {
      ++(tbl.bodyPos.body_crd.km[i]);
      mm[i] -= 1000000;
    }
    while (mm[i] <= -1000000) {
      --(tbl.bodyPos.body_crd.km[i]);
      mm[i] += 1000000;
    }

    while (mm[i] >= 1000) {
      ++(tbl.bodyPos.body_crd.m[i]);
      mm[i] -= 1000;
    }
    while (mm[i] <= -1000) {
      --(tbl.bodyPos.body_crd.m[i]);
      mm[i] += 1000;
    }
    tbl.bodyPos.cnt_integer[i] = mm[i] * tbl.bodyPos.cnt_per_m / 1000;
  }

  // 角度の更新
  long pre_div16_cnt = tbl.bodyPos.div16_cnt;
  tbl.bodyPos.div16_cnt =
    (static_cast<long>(0x10000 * position.zt.to_rad() / (2.0 * M_PI))
     << (DIV16_MUL_SHIFT - 16)) / tbl.bodyPos.div16_cnt_mul;
  if (pre_div16_cnt + (tbl.bodyPos.div16_cnt_max>>1) < tbl.bodyPos.div16_cnt) {
    --(tbl.bodyPos.rotate_num);
  } else if (pre_div16_cnt - (tbl.bodyPos.div16_cnt_max >> 1) <
             tbl.bodyPos.div16_cnt) {
    ++(tbl.bodyPos.rotate_num);
  }
}


VXV::Position3D tRunCtrl_Simulator::getBodyPosition(const VXV::Position3D&
                                                    position) {
  // 筐体位置を返す
  int x_mm, y_mm;
  get_mmCoordinate(&tbl.gl_crd, &x_mm, &y_mm);
  return VXV::Position(x_mm, y_mm,
                       VXV::Direction::rad(2.0 * M_PI * tbl.gl_crd.div16
                                           / 0x10000));
}