2017-10

スポンサーサイト

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。

ルンバプログラミング 制御プログラム公開 (C++)

ルンバ制御用のプログラムがそれなりのものに仕上がったので公開します。
Roomba Open Interface の仕様書を読みながら、C++で形にしました。
参考にした仕様書は以下の二点。
簡易仕様書
詳細な仕様書(こちらを主に使用)

ルンバの直進距離や回転角度周りの値がおかしいため、制御プログラムの内部で補正しています。
距離値と角度値がおかしい場合は、
RoombaController.cpp内の定数(DISTANCE_CRRECTION_VALUE、ANGLE_CRRECTION_VALUE)で補正してください。

サンプルプログラムは以下のものになります。
wiimoteのヌンチャクを使ってルンバを動かすプログラムです。

実際に動かした結果はこんな感じ。


以下サンプルコード

#include
#include
#include "wiimote.h"
#include "RoombaController.h"
#include "Vector2d.h"

using namespace mylib;
typedef Vector2d vec;

vec calcCrossPoint(vec a1, vec a2, vec b1, vec b2) {
// ベクトルを初期化
vec va = a2 - a1;
vec vb = b2 - b1;
vec vc = b1 - a1;

// 交点の計算
vec ret = a1 + va * vb.cross(vc) / vb.cross(va);
return ret;
}

int main() {
wiimote wm = wiimote();
while(!wm.Connect(wiimote::FIRST_AVAILABLE)) ;
wm.SetReportType(wiimote::IN_BUTTONS_ACCEL_IR_EXT); // no IR dots
std::cerr << "wiimote conected!!" << std::endl;

roomba::Controller controller;
controller.initialize( "COM4" );

while( !wm.Button.Home() )
{
// 更新されるまで停止
while( wm.RefreshState() == NO_CHANGE ) Sleep(1);

if( wm.Button.A() ) controller.motors( true, true, true );
else controller.motors(false,false,false);

// wiiコントローラを使った操作
if( controller.renewSensorData() && wm.Button.B() ) {
// センサデータ取得用のクラス作成
roomba::SensorData sd = controller.getSensorData();

// 距離データ出力
std::cerr << sd.pose.distance << std::endl;

// 移動処理
float x = wm.Nunchuk.Joystick.X;
float y = wm.Nunchuk.Joystick.Y;
if( fabs(x) < 0.1 ) x = 0.0;
if( fabs(y) < 0.1 ) y = 0.0;

float v = std::sqrt(x*x + y*y) * 200.0;
v *= (y>0.0) ? 1.0 : -1.0;
vec target(x,y);
vec center = target * 0.5 * 100.0;
vec vertical = target.rotate( M_PI/2 );
vertical.normalize();

vec cross = calcCrossPoint( vec(0,0), vec(1,0), center, center+vertical );
double dist = std::sqrt(cross.x*cross.x + cross.y*cross.y);
dist *= (cross.x > 0.0) ? -1.0 : 1.0;

controller.drive( v, static_cast(dist) );
}
else {
controller.drive( 0, 0 );
}
}

wm.Disconnect();
return 0;
}

作成したプログラムは以下の場所に公開しておきます。
一応はLinuxでも動作するように作成したはずなので活用してやってください。
ルンバ制御プログラム

参考サイト:
Roomba OI (Open Interface) (旧SCI (Serial Control Interface))の勉強

追記:
2011/12/07 バグフィクスと機能の追加を行ないました。

● COMMENT FORM ●


管理者にだけ表示を許可する

トラックバック

http://chicklab.blog84.fc2.com/tb.php/34-3ba09ab7
この記事にトラックバックする(FC2ブログユーザー)

NEW ENTRY «  | BLOG TOP |  » OLD ENTRY

カレンダー

09 | 2017/10 | 11
1 2 3 4 5 6 7
8 9 10 11 12 13 14
15 16 17 18 19 20 21
22 23 24 25 26 27 28
29 30 31 - - - -

FC2カウンター

最新記事

最新コメント

最新トラックバック

月別アーカイブ

カテゴリ

未分類 (7)
日記 (7)
wiiremote (1)
google (1)
回路 (3)
psp (1)
programing (15)
linux (0)
OpenWRT (3)
kinect (1)
Raspberry Pi (8)

検索フォーム

RSSリンクの表示

リンク

このブログをリンクに追加する

google adsense

上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。