public float Distance(HalRobotMotion target)
{
var vec = target.Pose.Xyzwpr - this.Pose.Xyzwpr;
var distnace = vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2];
return((float)Math.Sqrt(distnace));
}
public int HalMoveStraightAsyn(HalRobotMotion motion)
{
this.HalStopProgram();
if (!this.ldd.ExecutePNS("PNS0101"))
{
throw new Exception("Start PNS0101 Fail");
}
var corJ = 0;
var OfsOrPos = 0;
float[] PosArray;
switch (motion.MotionType)
{
case HalRobotEnumMotionType.Offset:
corJ = 0; OfsOrPos = 0; PosArray = motion.ToXyzwprArray(); break;
case HalRobotEnumMotionType.Position:
corJ = 0; OfsOrPos = 1; PosArray = motion.ToXyzwprArray(); break;
case HalRobotEnumMotionType.Joint:
corJ = 1; OfsOrPos = 0; PosArray = motion.ToJointArray(); break; //Joint模式讀取J1~J6座標
default:
corJ = 0; OfsOrPos = 0; PosArray = motion.ToXyzwprArray(); break;
}
return(this.ldd.Pns0101MoveStraightAsync(PosArray, corJ, OfsOrPos, motion.IsTcpMove, motion.Speed));
}
public HalRobotMotion HalGetPose()
{
var robotInfo = this.ldd.GetCurrRobotInfo();
var motion = new HalRobotMotion()
{
MotionType = HalRobotEnumMotionType.None,//取得資料不會有移動類型
UserFrame = robotInfo.UserFrame,
UserTool = robotInfo.UserTool,
X = robotInfo.x,
Y = robotInfo.y,
Z = robotInfo.z,
W = robotInfo.w,
P = robotInfo.p,
R = robotInfo.r,
J1 = robotInfo.j1,
J2 = robotInfo.j2,
J3 = robotInfo.j3,
J4 = robotInfo.j4,
J5 = robotInfo.j5,
J6 = robotInfo.j6,
};
return(motion);
}
public int HalMoveStraightAsyn(HalRobotMotion motion)
{
return(0);
}