mRunCmd.cpp

/*
  RunCtrl の走行コマンドのモニタ用実装
  Satofumi KAMIMURA
  $Id$
*/

#include "mRunCtrl.h"
#include "parseArgs.h"
#include "screenTask.h"


void mRunCtrl::writeLocalPosition(const VXV::Position3D& ret_pos,
                                  unsigned long ticks) {
  mon->log->lock();
  mon->log->writeTag("run", "getLocalPosition", ticks);
  fprintf(mon->log->fd, " timestamp=%lu x=%d y=%d div16=%d",
          crd_ticks, ret_pos.x, ret_pos.y, to_div16(ret_pos.zt));
  mon->log->writeTagEnd();
  mon->log->unlock();
}


void mRunCtrl::readLocalPosition(VXV::Position& ret_pos) {
  mon->log->lock();
  unsigned long ticks = mon->log->readTag("run", "getLocalPosition");

  const char* line = mon->log->getLineBuffer();
  long div16 = 0;
  sscanf(line, "%*s %*s timestamp=%lu x=%d y=%d div16=%ld",
         &crd_ticks, &ret_pos.x, &ret_pos.y, &div16);
  ret_pos.zt = rad(2.0 * M_PI * div16 / 0x10000);
  crd_position = ret_pos;
  mon->log->unlock();

  mon->task->waitToTicks(ticks, cond, mutex);
}


void mRunCtrl::writeIsStable(bool ret_value, unsigned long ticks) {
  mon->log->lock();
  mon->log->writeTag("run", "isStable", ticks);
  fprintf(mon->log->fd, " ret_value=%d", (ret_value) ? 1 : 0);
  mon->log->writeTagEnd();
  mon->log->unlock();
}


bool mRunCtrl::readIsStable(void) {
  mon->log->lock();
  unsigned long ticks = mon->log->readTag("run", "isStable");

  const char* line = mon->log->getLineBuffer();
  int ret_value = 0;
  sscanf(line, "%*s %*s ret_value=%d", &ret_value);
  mon->log->unlock();
  mon->task->waitToTicks(ticks, cond, mutex);

  return (ret_value == 0) ? false : true;
}


VXV::Position3D mRunCtrl::getLocalPosition(void) {
  VXV::Position ret_pos;

  if (MonitorMode == Monitor::Playback) {
    // ログからの読み出し
    readLocalPosition(ret_pos);

  } else {
    // ログへの書き込み
    ret_pos = RunCtrl::getLocalPosition();
    writeLocalPosition(ret_pos, mon->getTicks());
  }
  if (MonitorMode != Monitor::Simulator) {
    simulator->setLocalPosition(ret_pos - local_offset);
  }

  return ret_pos;
}


void mRunCtrl::stop(void) {
  // メイン画面への経路表示
  // !!!

  mon->task->waitOnlyCommand(mon, MonitorMode, cond, mutex, "run", "stop");
  RunCtrl::stop();

  // コマンド履歴への描画
  mon->scr->commandLogPrintf("stop();\n");
}


void mRunCtrl::followLine(const VXV::Position& position,
                          const CoordinateCtrl* crd) {
  // メイン画面への経路表示
  // !!!

  mon->task->waitOnlyCommand(mon, MonitorMode, cond, mutex,
                             "run", "followLine");
  RunCtrl::followLine(position, crd);

  // コマンド履歴への描画
  mon->scr->commandLogPrintf("followLine(%d, %d, %d);\n",
                                position.x, position.y, position.zt.to_deg());
}


void mRunCtrl::followCircle(const VXV::Grid& center, int radius,
                            const CoordinateCtrl* crd) {
  // メイン画面への経路表示
  // !!!

  mon->task->waitOnlyCommand(mon, MonitorMode, cond, mutex,
                             "run", "followCircle");
  RunCtrl::followCircle(center, radius, crd);

  // コマンド履歴への描画
  mon->scr->commandLogPrintf("followCircle((%d, %d), %d);\n",
                                center.x, center.y, radius);
}


void mRunCtrl::followCircleOnTangent(const VXV::Position& position, int radius,
                                     const CoordinateCtrl* crd) {
  RunCtrl::followCircleOnTangent(position, radius, crd);
}


void mRunCtrl::stopToLine(const VXV::Position& position,
                          const CoordinateCtrl* crd) {
  // メイン画面への経路表示
  // !!!

  mon->task->waitOnlyCommand(mon, MonitorMode, cond, mutex,
                             "run", "stopToLine");
  RunCtrl::stopToLine(position, crd);

  // コマンド履歴への描画
  mon->scr->commandLogPrintf("stopToLine(%d, %d, %d);\n",
                                position.x, position.y, position.zt.to_deg());
}


void mRunCtrl::rotateToDirection(const VXV::Direction& direction,
                                 const CoordinateCtrl* crd) {
  // メイン画面への経路表示
  // !!!

  mon->task->waitOnlyCommand(mon, MonitorMode, cond, mutex,
                             "run", "rotateDirection");
  RunCtrl::rotateToDirection(direction, crd);

  // コマンド履歴への描画
  mon->scr->commandLogPrintf("rotateToDirection(%d);\n",direction.to_deg());
}


void mRunCtrl::rotateAngle(const VXV::Direction& direction) {
  // メイン画面への経路表示
  // !!!

  mon->task->waitOnlyCommand(mon, MonitorMode, cond, mutex,
                             "run", "rotateAngle");
  RunCtrl::rotateAngle(direction);

  // コマンド履歴への描画
  mon->scr->commandLogPrintf("rotateAngle(%d);\n", direction.to_deg());
}


void mRunCtrl::spin(const VXV::Direction& velocity) {
  // メイン画面への経路表示
  // !!!

  mon->task->waitOnlyCommand(mon, MonitorMode, cond, mutex, "run", "spin");
  RunCtrl::spin(velocity);

  // コマンド履歴への描画
  mon->scr->commandLogPrintf("spin(%d);\n", velocity.to_deg());
}


void mRunCtrl::lastMoveCommand(const CoordinateCtrl* crd) {
  mon->task->waitOnlyCommand(mon, MonitorMode, cond, mutex,
                             "run", "lastMoveCommand");
  RunCtrl::lastMoveCommand(crd);

  // コマンド履歴への描画
  mon->scr->commandLogPrintf("lastMoveCommand()\n");
}


bool mRunCtrl::isStable(void) {

  if (MonitorMode == Monitor::Playback) {
    // ログからの読み出し
    return readIsStable();

  } else {
    // ログへの書き込み
    bool ret_value = RunCtrl::isStable();
    writeIsStable(ret_value, mon->getTicks());

    return ret_value;
  }
}


void mRunCtrl::pauseLock(void) {
  if (MonitorMode == Monitor::Playback) {
    SDL_SemWait(mon->pause_sem);
  }
}


void mRunCtrl::pauseUnlock(void) {
  if (MonitorMode == Monitor::Playback) {
    SDL_SemPost(mon->pause_sem);
  }
}


void mRunCtrl::servoCtrl(bool on) {
  pauseLock();
  RunCtrl::servoCtrl(on);
  pauseUnlock();

  // コマンド履歴への描画
  mon->scr->commandLogPrintf("servoCtrl(%s)\n", on ? "true" : "false");
}


void mRunCtrl::setMotorPwm(int id, unsigned char duty) {
  pauseLock();
  RunCtrl::setMotorPwm(id, duty);
  pauseUnlock();
}


void mRunCtrl::setMotorPwm(int id, unsigned char duty[], int nums) {
  pauseLock();
  RunCtrl::setMotorPwm(id, duty, nums);
  pauseUnlock();
}


void mRunCtrl::setMotorMode(int id, unsigned char mode) {
  pauseLock();
  RunCtrl::setMotorPwm(id, mode);
  pauseUnlock();
}


void mRunCtrl::setMotorMode(int id, unsigned char mode[], int nums) {
  pauseLock();
  RunCtrl::setMotorPwm(id, mode, nums);
  pauseUnlock();
}


#if 0
void mRunCtrl::getEncoderVel(int id, int *cnt) {
  RunCtrl::getEncoderVel(id, cnt);
}


void mRunCtrl::getEncoderVel(int id, int cnt[], int nums) {
  RunCtrl::getEncoderVel(id, cnt, nums);
}


void mRunCtrl::getEncoderValue(int id, unsigned short *cnt) {
  RunCtrl::getEncoderValue(id, cnt);
}


void mRunCtrl::getEncoderValue(int id, unsigned short cnt[], int nums) {
  RunCtrl::getEncoderValue(id, cnt, nums);
}
#endif


void mRunCtrl::setWheelVel(int id, int mm_vel) {
  pauseLock();
  RunCtrl::setWheelVel(id, mm_vel);
  pauseUnlock();
}


void mRunCtrl::setWheelVel(int id, int mm_vel[], int nums) {
  pauseLock();
  RunCtrl::setWheelVel(id, mm_vel, nums);
  pauseUnlock();
}