inputHandle.cpp

/*
  入力をロボットの移動速度に変換
  Satofumi KAMIMURA
  $Id$
*/

#include "inputHandle.h"
#include "vutils.h"
#include "timeUtils.h"


void drawJetMode(const robotInput_t& ri) {
  static bool pre_jet = false;
  static unsigned long draw_index = 0;

  if (ri.jet != pre_jet) {
    pre_jet = ri.jet;
    if (ri.jet) {
      // 描画
      draw_index = vmonitor::drawText("Jet !", VXV::Grid(10, 10));
    } else {
      // 削除
      vmonitor::clear(draw_index);
    }
  }
}


void drawCaptures(URGCtrl& urg, RunCtrl& run) {
  static TicksPosition ticksPos;
  static unsigned long pre_index = 0;

  ticksPos.add(run.crd_position, run.crd_ticks);

  urg.convert(ticksPos);
  vmonitor::clear(pre_index);
  pre_index = vmonitor::drawPoints(urg.crd_points, Red);

  ticksPos.del_olderThan(urg.crd_ticks - 5000);
}


// 加速度を考慮した速度計算
static double setVelocity(double now, double ref,
                          double RefAcc, long diff) {
  double sign = ref - now;
  double add = RefAcc * diff / 1000.0;
  double value = now + ((sign > 0.0) ? +add : -add);
  if (sign > 0.0) {
    if (value > ref) {
      value = ref;
    }
  } else {
    if (value < ref) {
      value = ref;
    }
  }
  return value;
}


// 入力の取得
robotInput_t inputHandle(const JoystickCtrl& js,
                         const UserInput::userInput_t& key) {
  static unsigned long pre_ticks = VXV::GetTicks();
  static double now_straight = 0;
  static double now_rotate = 0.0;
  const double RotateRef = (2.0 * M_PI) / 4.0;
  const double RotateAcc = (2.0 * M_PI) * 3.0 / 4.0;
  const int StraightRef = 300;
  const int StraightAcc = 300;

  unsigned long ticks = VXV::GetTicks();
  double ref_straight = 0;
  double ref_rotate = 0.0;
  robotInput_t ri;
  ri.jet = false;

  // キー入力
  if (key.nowPressedCode(SDLK_RSHIFT) || key.nowPressedCode(SDLK_LSHIFT)) {
    ref_straight = StraightRef;
  }
  if (key.nowPressedCode(SDLK_a)) {
    ref_rotate = RotateRef;
  }
  if (key.nowPressedCode(SDLK_s)) {
    ref_rotate -= RotateRef;
  }
  if (key.nowPressedCode(SDLK_RETURN)) {
    ri.jet = true;
  }

  // ジョイスティック入力
  if (js.isActivated()) {
    int js_x = -js.getAxisValue(0);
    ref_rotate += (js_x > 3200) ? RotateRef : (js_x < -3200) ? -RotateRef : 0;

    if (js.getButtonValue(0)) {
      // 並進移動(前進)
      ref_straight = StraightRef;
    } else if (js.getButtonValue(3)) {
      ref_straight = -StraightRef;
    }
    if (js.getButtonValue(6) || js.getButtonValue(7)) {
      // 並進速度を倍にする
      ri.jet = true;
    }
  }
  if (ri.jet) {
    ref_straight *= 2.0;
  }
  long diff = ticks - pre_ticks;
  pre_ticks = ticks;
  now_straight = setVelocity(now_straight, ref_straight, StraightAcc, diff);
  now_rotate = setVelocity(now_rotate, ref_rotate, RotateAcc, diff);

  ri.straight = static_cast<int>(now_straight);
  ri.rotate = VXV::Direction::rad(now_rotate);
  return ri;
}


// ロボットの制御
void robotCtrl(mRunCtrl& run, const robotInput_t& ri) {
  enum { RIGHT = 0, LEFT = 1 };

  // 並進速度、回転速度、トレッドから車輪毎の目標回転速度を計算
  int whl_mm_vel[2];
  int rotate_mm_vel = static_cast<int>(BODY_TREAD_MM * ri.rotate.to_rad() / 2);
  whl_mm_vel[RIGHT] = ri.straight + rotate_mm_vel;
  whl_mm_vel[LEFT] = ri.straight - rotate_mm_vel;

  for (int i = 0; i < 2; ++i) {
    run.setWheelVel(i, whl_mm_vel[i]);
  }
}