2011/08/28(日)PCからRS405CB/RS406CBを制御する
2011/08/28 13:08
いろんなパラメータがあって安全装置も入っているこの素敵サーボをJavaとPythonで制御するライブラリを書いたので，サーボの使い方と一緒に置いておきます．
配線
RS405CB/RS406CB取扱説明書より
数珠繋ぎは6つまでということに注意です．
サーボにIDを振る
このサーボはサーボ同士を繋げていって一本の配線で6つのサーボまでを制御することができます．そのためにはそれぞれのサーボにIDを付けてあげなければいけないので，その作業を行います．
まずはこちらのサンプルプログラムからVB版をダウンロードします．
ソフトを起動して，電池や安定化電源の電源を入れた後，以下の手順を実行します．
- RSC-U485のささっているポートを選ぶ(コントロールパネル>システム>デバイスマネージャで確認できる)
- 画像2のボタンを押す(自動で設定が読み込まれるはず)
- 適当なサーボIDを選択し，setボタンを押す
- 画像4のボタンを押し，書き込みが終了した旨のメッセージを待つ
設定やパラメータなどもここから弄れます．
プログラム
試したい盛りでgithubなんぞ使ってますが，いまひとつ操作に慣れてない感じですね．RSC_U485_Java
RSC_U485_Python
あらかじめ，Java版の場合はrxtxを，Python版の場合はpyserialをインストールして置いてください．
クラスの使い方はコードに入ってるドキュメントを見れば大体わかると信じてます．
Java版だとこういった感じで使用します．
public static void main(String[] args) {
RSC_U485 servo = new RSC_U485("COM17", 115200);
System.out.println("ID1のサーボのトルクをオン");
servo.torque(1, true);
System.out.println("最高速度で100度の位置へ回転");
servo.move(1, 1000, 0);
try {
Thread.sleep(500);//しばし待つ
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
System.out.println("現在角度:"+servo.getAngle(1));
System.out.println("1秒かけて0度の位置へ");
servo.move(1, 0, 100);
try {
Thread.sleep(1000);//しばし待つ
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
System.out.println("現在角度:"+servo.getAngle(1));
}
ちなみにJavaとPythonで書いたのはWindowsからもUbuntuからも制御したかったのが理由なのですが，UbuntuでRSC-U485を認識させる
2012/1/13追記この記事のコメントで,以下のようにすることでUbuntuでもRSC-U485が認識することをまえださんから教えていただきました.
ありがとうございます!
Ubuntu 9.10 では RS-U485 を
# modprobe ftdi_sio vendor=0x1115 product=0x0008
で認識しました．
資料
説明書サンプルプログラム
2011/08/01(月)Javaでシリアルポートの詳細な名前取得
2011/08/01 19:29
システムの色んな情報を受け取れるらしいWMIをJavaにラップしているjWMIをeclipseで使う，という話です．
使用手順
- パッケージエクスプローラ上の任意のJavaプロジェクトを右クリック
- 「新規>パッケージ」を選択
- 名前に「com.citumpe.ctpTools」と入力
- ここからダウンロードしてきたフォルダの中の「jWMI.java」をさっき作ったパッケージの中に放り込む
- 何か341行目が化けてるので「OSRecoveryConfiguracion」みたいな感じで直す
import com.citumpe.ctpTools.jWMI;とインポートし，
try {
// COMポートの詳細な名前を教えてもらう
String name = jWMI.getWMIValue(
"Select Caption from Win32_SerialPort", "Caption");
System.out.println(name);
} catch (Exception e) {
// TODO 自動生成された catch ブロック
e.printStackTrace();
}
という風に使ったら，通信ポート (COM1) Silicon Labs CP210x USB to UART Bridge (COM33) URG Series USB Device Driver (COM31)てな感じで返ってきた…んですけど，モノによって出てこないものがあります．
確認しているものだとmbed(mbed Serial Port)とft232シリアル変換IC(USB Serial Port)は取得できなかったです．何故だ…．
2011/8/26追記
name = jWMI.getWMIValue("Select Caption from Win32_PnPEntity", "Caption");
としたら，今まででてこなかったものも表示されました．必要ない情報も多くなってしまいますけれど．
参考
シリアルポートの名前取得2011/07/09(土)QPToolkitでマーカの位置姿勢計測したメモ
2011/07/10 00:36
QPToolkitとは最近すっかり社会的認知度が高まっているARを行うためのライブラリから位置計測部分を抜き出した位置計測フレームワークです．
このフレームワークを利用することで,ごく普通のWebカメラから紙に印刷したマーカまでの距離姿勢を簡単に計測することができるのです．
カメラからマーカまでの3次元上の距離を計測するところまでは値を見るだけで簡単にできたのですが，姿勢を計測するのに結構手間取ってしまいました．
今回はそのことについて，もしかしたら超常識的なことなのかもしれないメモ書きです．
姿勢計測を行う
マーカの姿勢を取得するには回転行列＋並進ベクトルで表されているOpenGLの行列というものを扱えばいいようです．併進ベクトルはカメラからマーカまでの距離として，回転成分はなんぞや，と思って調べてみるとARマーカの回転成分はx-y-zオイラー角回転行列で示されているという文章を見つけました．
これは三次元の回転行列をx,y,z軸の順で回転させたもので，この形式の回転行列であるということがわかれば一応角度を取り出すことができます．
ここから先の計算に関してはこちらの下の方にそのものずばりが書いてありました．手で計算した後に見つけたって言う…．
ここまでわかればプログラムに落とすことができますね．
processingで書いたものを以下に示してみます．プロトタイピングってことでとっても縦長です．すいません．
プログラム
QPServer上で「Name」と「OpenGL model view matrix～」の二つにチェックを入れた状態で動かしてください．画面上に表示された3Dがカメラ上のマーカにあわせて傾いたり近づいたりするはずです．16cm四方のマーカならそのままで大丈夫ですが，それ以外のサイズだとマーカのサイズに合わせて表示位置や立体のサイズ変えないといけないかもしれません．
あとマーカを見つけたときだけ表示というようにしているせいか凄いちらつきます．画面から離れて部屋を明るくしてこのプログラムを利用してください．
import processing.net.*;
Client myClient;
double phi,theta,psi;
double X,Y,Z;
double[][] matrix = new double[4][4];
void setup() {
myClient = new Client(this,"192.168.145.1",55555); // IPとポート番号
size(480, 480, P3D);
frameRate(30);
}
void draw() {
background(0);
if (myClient.available() > 0) {
String dataIn = myClient.readStringUntil('\n');
if ( dataIn != null ) {
// "Num="を含むデータ列かどうかをチェックし，データ数を取得
if ( dataIn.indexOf("Num=")==0 ) {
int num = int(split(trim(dataIn),' ' )[1]);
// num個のマーカのデータを取得する
for (int i=0; i<num; i++) {
String marker_info = myClient.readStringUntil('\n');
if ( marker_info!= null ) {
// １行分のデータを切り分けて欲しいデータを取得する
String[] data = split(trim(marker_info),',');
// マーカの名前と２次元座標
String name = data[0];
for(int m=0;m<4;m++)
{
for(int n=0;n<4;n++)
{
matrix[n][m] = Double.parseDouble(data[1+m*4+n]);
}
}
//傾斜角度を求める
if (matrix[0][2] > 0.998)
{
phi = Math.atan2(matrix[1][0],matrix[1][1]);
theta = Math.PI/2.0;
psi = 0;
}
else if (matrix[0][2] < -0.998)
{
phi = Math.atan2(matrix[1][0],matrix[1][1]);
theta = -Math.PI/2.0;
psi = 0;
}
else
{
phi = Math.atan2(-matrix[1][2],matrix[2][2]);
theta = Math.asin(matrix[0][2]);
psi = Math.atan2(-matrix[0][1],matrix[0][0]);
}
//併進ベクトルを代入
X=matrix[0][3];
Y=matrix[1][3];
Z=matrix[2][3];
println("X:"+X+" Y:"+Y+" Z:"+Z);
println("phi:"+Math.toDegrees(phi)+" theta"+Math.toDegrees(theta)+" psi:"+Math.toDegrees(psi));
println();
//マーカの位置にあわせて立体を表示
translate(width/2+(int)X, height/2+(int)Y,-(int)Z+600);
//軸の向き的にy軸に180度，z軸に90度回転させて表示．あとなんか-をかける．
rotateX(-(float)phi);
rotateY(-((float)theta+(float)Math.PI));
rotateZ(-((float)psi+(float)Math.PI/2));
//マーカ平面ぽいものを描画
stroke(0);
fill(255, 255, 255);
pushMatrix();
translate(-50, -50, 0);
rect(0,0,100,100);
popMatrix();
//X軸の棒を描画
stroke(0);
fill(255, 0, 0);
pushMatrix();
translate(-50, 0, 0);
box(100, 5, 5);
popMatrix();
//Y軸の棒を描画
stroke(0);
fill(0, 255, 0);
pushMatrix();
translate(0, -50, 0);
box(5, 100, 5);
popMatrix();
//Z軸の棒を描画
stroke(0);
fill(0, 0, 255);
pushMatrix();
translate(0, 0, 50);
box(5, 5, 100);
popMatrix();
}
}
}
}
myClient.clear();
}
}