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