/// <summary> /// 現在のPHANTOM手先速度を返します /// </summary> /// <returns>速度ベクトル [mm/s]</returns> public Vector3D GetVelocity() { double[] velocity = new double[3] { 0, 0, 0 }; Hd.hdGetDoublev(Hd.ParameterName.HD_CURRENT_VELOCITY, velocity); return(new Vector3D(velocity)); }
/// <summary> /// 現在のPHANTOMジンバル姿勢を返します /// <remarks>手先の姿勢ではありません。ジンバル部エンコーダの値です。</remarks> /// </summary> /// <returns>ジンバル部分の内、根元からペン部にかけて X~Z に対応した角度 [rad]</returns> public Vector3D GetGimbalAngles() { double[] gimbals = new double[3] { 0, 0, 0 }; Hd.hdGetDoublev(Hd.ParameterName.HD_CURRENT_GIMBAL_ANGLES, gimbals); return(new Vector3D(gimbals)); }
/// <summary> /// 現在のPHANTOM手先座標を返します /// </summary> /// <returns>位置ベクトル [mm]</returns> public Vector3D GetPosition() { double[] position = new double[3] { 0, 0, 0 }; Hd.hdGetDoublev(Hd.ParameterName.HD_CURRENT_POSITION, position); return(new Vector3D(position)); }
/// <summary> /// 同期的に処理を呼び出します /// </summary> public void Do(Callback callback) { Hd.hdScheduleSynchronous( (data) => { return(DoCallback(callback)); }, IntPtr.Zero, Hd.Priority.HD_DEFAULT_SCHEDULER_PRIORITY ); ErrorCheck("ScheduleSynchronous"); }
/// <summary> /// 登録済の非同期実行処理を全て消去します /// </summary> public void ClearSchedule() { foreach (uint handle in ScheduleHandles) { Hd.hdUnschedule(handle); ErrorCheck("Unschedule #" + handle.ToString()); } ScheduleHandles.Clear(); CallbackMethods.Clear(); }
/// <summary> /// コールバックメソッド呼び出しをより簡略化するためのラップ /// </summary> /// <param name="callback">引数無しでboolを返すだけに簡略化したメソッド</param> /// <returns>完了したか</returns> private Hd.CallbackResult DoCallback(Callback callback) { bool result; Hd.hdBeginFrame(hHD); result = callback(); Hd.hdEndFrame(hHD); return(result ? Hd.CallbackResult.HD_CALLBACK_CONTINUE : Hd.CallbackResult.HD_CALLBACK_DONE); }
/// <summary> /// デバイスの使用を終了し、切断します /// </summary> public void Close() { Stop(); ClearSchedule(); if (hHD != (uint)Hd.DeviceHandle.HD_INVALID_HANDLE) { Hd.hdDisableDevice(hHD); ErrorCheck("Disable device"); } }
/// <summary> /// ペン先端の座標を返します /// </summary> /// <returns>ペン先端座標 [mm]</returns> public Vector3D GetTipPosition() { double[] position = new double[3] { 0, 0, 0 }; double[] matrix = new double[16]; Hd.hdGetDoublev(Hd.ParameterName.HD_CURRENT_POSITION, position); Hd.hdGetDoublev(Hd.ParameterName.HD_CURRENT_TRANSFORM, matrix); return(new Vector3D( (position[0] + matrix[0] * TipOffset.X + matrix[4] * TipOffset.Y + matrix[8] * TipOffset.Z), (position[1] + matrix[1] * TipOffset.X + matrix[5] * TipOffset.Y + matrix[9] * TipOffset.Z), (position[2] + matrix[2] * TipOffset.X + matrix[6] * TipOffset.Y + matrix[10] * TipOffset.Z) )); }
/// <summary> /// 非同期処理を停止します /// </summary> public void Stop() { if (!IsRunning) { return; } IsRunning = false; Hd.hdStopScheduler(); ErrorCheck("StopScheduler"); // 力も停止 Hd.hdDisable(Hd.Capability.HD_FORCE_OUTPUT); ErrorCheck("Disable force output"); }
/// <summary> /// デフォルトのデバイスに接続します /// </summary> public SimplePhantom() { IsRunning = false; // デフォルトのデバイスを準備 hHD = Hd.hdInitDevice(Hd.DeviceHandle.HD_DEFAULT_DEVICE); ErrorCheck(); // コールバックメソッドを保持するリスト CallbackMethods = new List <Hd.SchedulerCallback>(); // スケジューリングされたメソッドのハンドルをこれで保持 ScheduleHandles = new List <ulong>(); // 可動範囲を取得 LoadWorkspaceLimit(); }
/// <summary> /// 非同期実行にメソッドを追加します /// </summary> /// <param name="callback">要継続ならtrueを返すコールバックメソッド</param> public void AddSchedule(Callback callback) { Hd.SchedulerCallback method = (data) => { return(DoCallback(callback)); }; CallbackMethods.Add(method); ulong handle = Hd.hdScheduleAsynchronous( method, IntPtr.Zero, Hd.Priority.HD_DEFAULT_SCHEDULER_PRIORITY ); ErrorCheck("ScheduleAsynchronous"); ScheduleHandles.Add(handle); }
/// <summary> /// 非同期処理を開始します /// </summary> public void Start() { if (IsRunning) { return; } // 力を発生させるのは標準でON Hd.hdEnable(Hd.Capability.HD_FORCE_OUTPUT); ErrorCheck("Enable force output"); // 非同期処理も開始 Hd.hdStartScheduler(); ErrorCheck("Start scheduler"); IsRunning = true; }
/// <summary> /// 直前のHDAPI呼び出しでエラーがあれば、例外を発生させます /// </summary> /// <param name="situation">何をしていたかを伝える文字列</param> static private void ErrorCheck(string situation) { Hd.ErrorInfo error; if (Hd.IsError(error = Hd.hdGetError())) { string message = Hd.GetErrorString(error.ErrorCode); if (situation.Equals("")) { System.Diagnostics.Debug.WriteLine("HDAPI error : " + message); throw new HdApiException(message); } else { System.Diagnostics.Debug.WriteLine("HDAPI error : " + situation + " / " + message); throw new HdApiException(situation + " / " + message); } } }
/// <summary> /// 可動範囲を取得 /// </summary> private void LoadWorkspaceLimit() { double[] val = new double[6]; // 可動限界範囲を取得 Hd.hdGetDoublev(Hd.ParameterName.HD_MAX_WORKSPACE_DIMENSIONS, val); ErrorCheck("Getting max workspace"); WorkspaceMinimum = new Vector3D(val[0], val[1], val[2]); WorkspaceMaximum = new Vector3D(val[3], val[4], val[5]); // 推奨可動範囲を取得 Hd.hdGetDoublev(Hd.ParameterName.HD_USABLE_WORKSPACE_DIMENSIONS, val); ErrorCheck("Getting usable workspace"); UsableWorkspaceMinimum = new Vector3D(val[0], val[1], val[2]); UsableWorkspaceMaximum = new Vector3D(val[3], val[4], val[5]); // 机の高さを取得 float[] offset = new float[1]; Hd.hdGetFloatv(Hd.ParameterName.HD_TABLETOP_OFFSET, offset); ErrorCheck("Getting table-top offset"); TableTopOffset = (double)offset[0]; }
/// <summary> /// PHANTOM発揮力を設定します /// </summary> /// <param name="force">力のベクトル [N]</param> public void SetForce(Vector3D force) { double[] forceArray = force.ToArray(); Hd.hdSetDoublev(Hd.ParameterName.HD_CURRENT_FORCE, forceArray); }
/// <summary> /// サーボループ開始からの経過時間を取得します /// </summary> /// <returns>時間 [s]</returns> public double GetSchedulerTimeStamp() { return(Hd.hdGetSchedulerTimeStamp()); }
/// <summary> /// 現在押されているボタンを取得します /// </summary> /// <returns>Button1 | Button2 | Button3 | Button4</returns> public Buttons GetButton() { int[] button = new int[1]; Hd.hdGetIntegerv(Hd.ParameterName.HD_CURRENT_BUTTONS, button); return((Buttons)button[0]); }
/// <summary> /// 現在のPHANTOM手先変換行列を返します /// </summary> /// <returns>4 × 4 変換行列</returns> public Matrix GetTransformMatrix() { double[] value = new double[16]; Hd.hdGetDoublev(Hd.ParameterName.HD_CURRENT_TRANSFORM, value); return(new Matrix(value)); }
/// <summary> /// 処理が1秒間に何回呼ばれるかを指定します /// </summary> /// <param name="rate">周期 [Hz] 500 or 1000</param> public void SetSchedulerRate(uint rate) { Hd.hdSetSchedulerRate(rate); ErrorCheck(); }