/// <summary>
/// 作業を開始します
/// </summary>
public void WorkStart(double target_x, double target_y, SettingsObj obj)
{
// 作業をクリア
DobotDll.SetQueuedCmdClear();
// コマンド開始
DobotDll.SetQueuedCmdStartExec();
OnWorkEvent?.Invoke(this, "作業開始");
// 現在位置を取得
var pose = GetCurrentPose();
OnWorkEvent(this, $"現在位置を取得:");
OnWorkEvent?.Invoke(this, $"X:{pose.x}, Y:{pose.y}, Z:{pose.z}, R:{pose.rHead}");
// 対象の上部まで移動(Z座標は適当)
var cmdIndex = ptp((byte)2, (float)target_x, (float)target_y, (float)0.0, pose.rHead);
OnWorkEvent?.Invoke(this, "対象の上部まで移動:");
OnWorkEvent?.Invoke(this, $"X:{(float)target_x}, Y:{(float)target_y}, Z:{(float)0.0}, R:{pose.rHead}");
// 対象物のZ座標計算
var object_z = obj.PedestalZ + obj.ObjectHeight;
OnWorkEvent?.Invoke(this, "対象物のZ座標:");
OnWorkEvent?.Invoke(this, $"Z:{object_z}");
// 対象の位置まで下がる
cmdIndex = ptp((byte)2, (float)target_x, (float)target_y, (float)object_z, pose.rHead);
OnWorkEvent?.Invoke(this, "対象物までアームを降ろす:");
OnWorkEvent?.Invoke(this, $"X:{(float)target_x}, Y:{(float)target_y}, Z:{(float)object_z}, R:{pose.rHead}");
// 下がったらサクションカップONしてつかむ
DobotDll.SetEndEffectorSuctionCup(true, true, true, ref cmdIndex);
OnWorkEvent?.Invoke(this, "サクションカップでつかむ");
// いったん上に持ち上げる
cmdIndex = ptp((byte)2, (float)target_x, (float)target_y, (float)0.0, pose.rHead);
OnWorkEvent?.Invoke(this, $"持ち上げる:");
OnWorkEvent?.Invoke(this, $"X:{(float)target_x}, Y:{(float)target_y}, Z:{(float)0.0}, R:{pose.rHead}");
// 持っていく場所に移動
cmdIndex = ptp((byte)2, (float)obj.PlacePoseXCoordinate, (float)obj.PlacePoseYCoordinate, (float)obj.PlacePoseZCoordinate, pose.rHead);
OnWorkEvent?.Invoke(this, $"持っていく場所に移動:");
OnWorkEvent?.Invoke(this, $"X:{(float)obj.PlacePoseXCoordinate}, Y:{(float)obj.PlacePoseYCoordinate}, Z:{(float)obj.PlacePoseZCoordinate}, R:{pose.rHead}");
// 下がったらサクションカップOFFして放す
DobotDll.SetEndEffectorSuctionCup(false, false, false, ref cmdIndex);
OnWorkEvent?.Invoke(this, "サクションカップOFFして放す");
// 上がって終わり
cmdIndex = ptp((byte)2, (float)obj.PlacePoseXCoordinate, (float)obj.PlacePoseYCoordinate, (float)0.0, pose.rHead);
OnWorkEvent?.Invoke(this, "アームを上にあげる");
// コマンド終了
DobotDll.SetQueuedCmdStopExec();
OnWorkEvent?.Invoke(this, "作業終了");
}
