private void buttonGetPos_Click(object sender, RoutedEventArgs e)
{
try
{
double x, y, z, c;
SixAxisPose currentPos = MainWindow.m_MainCtrl._robotController.ROBOT_GetRobotPosition();
string msg = $"CurPos=>X:{currentPos.xAxis},Y:{currentPos.yAxis},Z:{currentPos.zAxis},rx:{currentPos.rxAxis},ry:{currentPos.ryAxis},rz:{currentPos.rzAxis},fig:{currentPos.fig}";
txtPos.Text = msg;
}
catch (Exception)
{
//提示报错信息
}
}
/// <summary>
/// 获取各默认(示教)位置
/// 注意调用该方法的时机
/// </summary>
/// <param name="enumDefaultPosition"></param>
/// <param name="slot">EnumDefaultPosition为trayA~trayE盘时才启作用</param>
/// <returns></returns>
private SixAxisPose GetThePosition(EnumDefaultPosition enumDefaultPosition, int slot = 0)
{
//TODO:需要限制调用此方法的时机?
if (enumDefaultPosition == EnumDefaultPosition.Catch)
{
var offsetX = GetSysStatus <double>(p => p.CatchOffsetX_Running);
var offsetY = GetSysStatus <double>(p => p.CatchOffsetY_Running);
var offsetAngle = GetSysStatus <double>(p => p.CatchOffsetAngle_Running);
var deltaAngleBetweenRobotAndCamera = GetSysConfig <double>(p => p.DeltaAngleBetweenRobotAndCamera);
//转成世界坐标系的偏差值
offsetX = offsetX * Math.Cos(deltaAngleBetweenRobotAndCamera * 3.1415926535 / 180.0) + offsetY * Math.Sin(deltaAngleBetweenRobotAndCamera * 3.1415926535 / 180.0);
offsetY = offsetY * Math.Cos(deltaAngleBetweenRobotAndCamera * 3.1415926535 / 180.0) - offsetX * Math.Sin(deltaAngleBetweenRobotAndCamera * 3.1415926535 / 180.0);
SixAxisPose position = new SixAxisPose();
position.xAxis = GetSysConfig <double>(p => p.RobotPose_Catch.xAxis) + offsetX;
position.yAxis = GetSysConfig <double>(p => p.RobotPose_Catch.yAxis) + offsetY;
position.zAxis = GetSysConfig <double>(p => p.RobotPose_Catch.zAxis);
position.rxAxis = GetSysConfig <double>(p => p.RobotPose_Catch.rxAxis);
position.ryAxis = GetSysConfig <double>(p => p.RobotPose_Catch.ryAxis);
position.rzAxis = GetSysConfig <double>(p => p.RobotPose_Catch.rzAxis) + offsetAngle;
position.fig = GetSysConfig <string>(p => p.RobotPose_Catch.fig);
return(position);
}
else if (enumDefaultPosition == EnumDefaultPosition.NG)
{
SixAxisPose position = new SixAxisPose();
position.xAxis = GetSysConfig <double>(p => p.RobotPose_NG.xAxis);
position.yAxis = GetSysConfig <double>(p => p.RobotPose_NG.yAxis);
position.zAxis = GetSysConfig <double>(p => p.RobotPose_NG.zAxis);
position.rxAxis = GetSysConfig <double>(p => p.RobotPose_NG.rxAxis);
position.ryAxis = GetSysConfig <double>(p => p.RobotPose_NG.ryAxis);
position.rzAxis = GetSysConfig <double>(p => p.RobotPose_NG.rzAxis);
position.fig = GetSysConfig <string>(p => p.RobotPose_NG.fig);
return(position);
}
else if (enumDefaultPosition == EnumDefaultPosition.StandyA)
{
SixAxisPose position = new SixAxisPose();
position.xAxis = GetSysConfig <double>(p => p.RobotPose_StandbyA.xAxis);
position.yAxis = GetSysConfig <double>(p => p.RobotPose_StandbyA.yAxis);
position.zAxis = GetSysConfig <double>(p => p.RobotPose_StandbyA.zAxis);
position.rxAxis = GetSysConfig <double>(p => p.RobotPose_StandbyA.rxAxis);
position.ryAxis = GetSysConfig <double>(p => p.RobotPose_StandbyA.ryAxis);
position.rzAxis = GetSysConfig <double>(p => p.RobotPose_StandbyA.rzAxis);
position.fig = GetSysConfig <string>(p => p.RobotPose_StandbyA.fig);
return(position);
}
else if (enumDefaultPosition == EnumDefaultPosition.StandyB)
{
SixAxisPose position = new SixAxisPose();
position.xAxis = GetSysConfig <double>(p => p.RobotPose_StandbyB.xAxis);
position.yAxis = GetSysConfig <double>(p => p.RobotPose_StandbyB.yAxis);
position.zAxis = GetSysConfig <double>(p => p.RobotPose_StandbyB.zAxis);
position.rxAxis = GetSysConfig <double>(p => p.RobotPose_StandbyB.rxAxis);
position.ryAxis = GetSysConfig <double>(p => p.RobotPose_StandbyB.ryAxis);
position.rzAxis = GetSysConfig <double>(p => p.RobotPose_StandbyB.rzAxis);
position.fig = GetSysConfig <string>(p => p.RobotPose_StandbyB.fig);
return(position);
}
else if (enumDefaultPosition == EnumDefaultPosition.TrayA)
{
SixAxisPose position = GetProductParam <SixAxisPose>(p => p.ProductInfo(EnumTrayType.TrayA, slot).RobotPose_Put);
return(position);
}
else if (enumDefaultPosition == EnumDefaultPosition.TrayB)
{
SixAxisPose position = GetProductParam <SixAxisPose>(p => p.ProductInfo(EnumTrayType.TrayB, slot).RobotPose_Put);
return(position);
}
else if (enumDefaultPosition == EnumDefaultPosition.TrayC)
{
SixAxisPose position = GetProductParam <SixAxisPose>(p => p.ProductInfo(EnumTrayType.TrayC, slot).RobotPose_Put);
return(position);
}
else if (enumDefaultPosition == EnumDefaultPosition.TrayD)
{
SixAxisPose position = GetProductParam <SixAxisPose>(p => p.ProductInfo(EnumTrayType.TrayD, slot).RobotPose_Put);
return(position);
}
else if (enumDefaultPosition == EnumDefaultPosition.TrayE)
{
SixAxisPose position = GetProductParam <SixAxisPose>(p => p.ProductInfo(EnumTrayType.TrayE, slot).RobotPose_Put);
return(position);
}
else
{
throw new Exception("无此类型位置数据");
}
}
