[[Spr1のHISパッケージ]]
*SPIDAR(3自由度版)サンプルプログラム [#w1146f06]
/src/HIS/samples/Spidar/Spidar.dsw
を開いてビルドしてみてください.
#include <iostream>
#include "His/HISpidar4.h"
#include <windows.h>
#include <conio.h>
// USB2.0アンプのドライバ
#include <Device/DRUsb20Simple.h>
#include <Device/DRUsb20Sh4.h>
// ContectのISAカードのドライバ.古いもの(1997-).
//#include "Device/DRContecIsaDa.h"
//#include "Device/DRContecIsaCounter.h"
//#include <Device/DRUsbH8Simple.h>
using namespace Spr; // SprはSpringheadライブラリの名前空間
using namespace std;
int main(){
//------------------------------------------------------------------------
// 初期化部分
//
DVDeviceManager devMan; // デバイスドライバを入れておく入れ物
devMan.RPool().Register(new DRUsb20Simple(10)); // USB2.0アンプ(ID 10)を登録
devMan.RPool().Register(new DRUsb20Sh4(0)); // SH4アンプ(ID 0)を登録
// IDはアンプ裏のロータリスイッチの値(0x0-0xF)です.
/* 使っていないドライバ.登録しておけば自動認識する.
devMan.RPool().Register(new DRUsbH8Simple(0));
devMan.RPool().Register(new DRContecIsaDa(0x300));
devMan.RPool().Register(new DRContecIsaCounter(0x200));
devMan.RPool().Register(new DRContecIsaCounter(0x210));
*/
bool bMinForce = true; // いきなり力を出すと危険なので,スペースを押すまでは最小張力を出力
const float K = 2000.0f; // ばね定数
/* ドライバの初期化.存在するデバイスに対応するバーチャルデバイスが
ここで初期化される */
devMan.Init();
cout << devMan; // デバイスの状態の表示
HISpidar4 spidar;
HISpidar4 spidar2;
// SPIDARの初期化.本来は,ハードウエア毎に HISpidar4 クラスを派生させるべき
// モータの取り付け位置. モータが直方体に取り付けられている場合は,
#define PX 0.265f // x方向の辺の長さ/2
#define PY 0.265f // y方向の辺の長さ/2
#define PZ 0.265f // z方向の辺の長さ/2
Vec3f motorPos[][4] = { // モータの取り付け位置(中心を原点とする)
{Vec3f(-PX, PY, PZ), Vec3f( PX, PY,-PZ), Vec3f(-PX,-PY,-PZ), Vec3f( PX,-PY, PZ)},
{Vec3f( PX, PY, PZ), Vec3f(-PX, PY,-PZ), Vec3f( PX,-PY,-PZ), Vec3f(-PX,-PY, PZ)}
};
cout.flush();
spidar.Init(devMan, motorPos[0], 0.3776f, 2.924062107079e-5f, 0.5f, 20.0f);
spidar2.Init(devMan, motorPos[1], 0.3776f, -2.924062107079e-5f, 0.5f, 20.0f);
cout.flush();
printf("Waiting...");
Sleep(2000);
printf("Calibration\n");
spidar.Calib();
spidar2.Calib();
//------------------------------------------------------------------------
// メインループ
//
while(1){
if (kbhit()){ // キー入力
int ch = getch();
switch(ch){
case ' ': // スペース:力覚ON/OFF
bMinForce = !bMinForce;
break;
case 0x1b: // ESC:終了
goto end;
break;
}
}
spidar.Update(0.001f); // アンプとの通信(力を提示・位置を取得)
Vec3f force; // 提示する力
if (spidar.GetPos().Y() < 0){ // 床の提示
force.Y() = - K * spidar.GetPos().Y();
}
if (force.norm() > 10){ // 大きすぎる力は提示できないので,10Nに制約
force = force.unit() * 10;
}
//------------------------------------------------------------------------
// 提示力の設定
if (bMinForce) spidar.SetMinForce();
else spidar.SetForce(force);
// 以下2番目のSPIDARについて同様に,
Vec3f force2;
spidar2.Update(0.001f);
if (spidar2.GetPos().Y() < 0){
force2.Y() = - K * spidar2.GetPos().Y();
}
if (bMinForce) spidar2.SetMinForce();
else spidar2.SetForce(force2);
#if 0 // デバッグ用:位置と力を表示
cout << "P:" << spidar.GetPos() << "\tF:" << spidar.GetForce() << std::endl;
#endif
#if 0 // デバッグ用:糸の長さを表示
for(int i=0; i<4; i++){
cout << spidar.Motor()[i].GetLength() << " ";
}
for(int i=0; i<4; i++){
cout << spidar2.Motor()[i].GetLength() << " ";
}
cout << std::endl;
cout.flush();
#endif
}
end:;
return 0;
}