wheelCtrl.cpp

/*
  並進速度と回転速度の指定による走行指示
  Satofumi KAMIMURA
  $Id$
*/

#if !MONITOR
#include <runCtrl.h>
#else
#include <mRunCtrl.h>
#endif
#include <vutils.h>
#include <stdio.h>
#include <stdlib.h>

using namespace VXV;


static void stop(RunInterface& run, unsigned long msec) {
  VXV::Delay(msec);
  run.stop();
  while ((abs(run.getStraightVel()) > 10) ||
         (abs(run.getRotateVel().to_deg()) > 10)) {
    VXV::Delay(100);
  }
}


static void setVelocityCtrl(RunCtrl& run,
                            int straight, const Direction& rotate) {
  enum { RIGHT = 0, LEFT = 1 };

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

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


int main(int argc, char *argv[]) {
  try {
#if !MONITOR
    RunCtrl run;
#else
    mRunCtrl run;
    vmonitor::show();
#endif
    if (run.connect(argc, argv) < 0) {
      printf("RunCtrl::connect: %s\n", run.what());
      exit(1);
    }

    setVelocityCtrl(run, 300, deg(0)); // 並進制御
    stop(run, 3000);

    setVelocityCtrl(run, 0, deg(45)); // 回転制御
    stop(run, 3000);

    setVelocityCtrl(run, 300, deg(45)); // 並進と回転の制御
    stop(run, 3000);

  } catch (std::exception& e) {
    printf("exception: %s\n", e.what());
  }
  return 0;
}