/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; }