/// <summary>
/// 坐标转换（倾斜状态）： 喷嘴距离基板高度 -> Z轴坐标
/// </summary>
/// <param name="needleBoard">距板高度</param>
/// <param name="boardHeight">基板测高值</param>
/// <returns></returns>
public static double NeedleBoard2Z(double needleBoard, double boardHeight, TiltType tiltType)
{
Log.Dprint("NeedleBoard2Z needleBoard=" + needleBoard + ", boardHeight=" + boardHeight
+ ", StandardZ=" + Machine.Instance.Robot.CalibPrm.StandardZ + ", StandardHeight=" + Machine.Instance.Robot.CalibPrm.StandardHeight);
if (tiltType == TiltType.NoTilt)
{
return(needleBoard + Machine.Instance.Robot.CalibPrm.StandardZ + (boardHeight - Machine.Instance.Robot.CalibPrm.StandardHeight));
}
else
{
List <AngleHeightPosOffset> curTiltCalibPrms = Machine.Instance.Robot.CalibPrm.AngleHeightPosOffsetList.FindAll(t => t.TiltType.Equals(tiltType));
if (curTiltCalibPrms.Count <= 0)
{
return(0);
}
AngleHeightPosOffset curTiltCalibPrm = curTiltCalibPrms.Find(t => t.Gap.Equals(needleBoard));
if (curTiltCalibPrm == null)
{
return(0);
}
else
{
if (curTiltCalibPrm.Gap <= 0.0001)
{
return(0);
}
else
{
return(curTiltCalibPrm.Gap + curTiltCalibPrm.StandardZ + (boardHeight - Machine.Instance.Robot.CalibPrm.StandardHeight));
}
}
}
}
/// <summary>
/// 相机坐标转换倾斜喷嘴坐标
/// </summary>
/// <param name="point"></param>
/// <param name="valveType"></param>
/// <param name="tiltType"></param>
/// <returns></returns>
public static PointD ToNeedle(this PointD point, ValveType valveType, TiltType tiltType)
{
PointD rtn = null;
if (valveType == ValveType.Valve1 || valveType == ValveType.Both)
{
if (tiltType == TiltType.NoTilt)
{
return(rtn = (point.ToSystem() + Machine.Instance.Robot.CalibPrm.NeedleCamera1 - Machine.Instance.Robot.CalibPrm.NeedleJet1).ToPoint().ToMachine());
}
AngleHeightPosOffset curTiltCalibPrm = Machine.Instance.Robot.CalibPrm.AngleHeightPosOffsetList.Find(t => t.TiltType.Equals(tiltType));
if (curTiltCalibPrm == null)
{
return(null);
}
rtn = (point.ToSystem() + curTiltCalibPrm.ValveCameraOffset - curTiltCalibPrm.DispenseOffset).ToPoint().ToMachine();
}
else
{
if (Machine.Instance.Setting.DualValveMode != DualValveMode.跟随)
{
rtn = new PointD(
point.X + Machine.Instance.Robot.CalibPrm.NeedleCamera2.X - Machine.Instance.Robot.CalibPrm.NeedleJet2.X + Math.Abs(Machine.Instance.Robot.PosA),
point.Y + Machine.Instance.Robot.CalibPrm.NeedleCamera2.Y - Machine.Instance.Robot.CalibPrm.NeedleJet2.Y + Math.Abs(Machine.Instance.Robot.PosB));
}
else
{
rtn = new PointD(
point.X + Machine.Instance.Robot.CalibPrm.NeedleCamera2.X - Machine.Instance.Robot.CalibPrm.NeedleJet2.X,
point.Y + Machine.Instance.Robot.CalibPrm.NeedleCamera2.Y - Machine.Instance.Robot.CalibPrm.NeedleJet2.Y);
}
}
return(rtn);
}
public TestTypeCoreectAngleForm()
{
InitializeComponent();
this.StartPosition = FormStartPosition.CenterScreen;
this.angleHeightPosOffset = new AngleHeightPosOffset();
this.Init();
this.UpdateByIndex();
this.FormClosed += TestTypeCoreectAngleForm_FormClosed;
}
/// <summary>
/// 胶阀倾斜到位
/// </summary>
/// <param name="tiltType"></param>
/// <param name="velU"></param>
/// <param name="accU"></param>
/// <returns></returns>
public Result ChangeValveTiltStatus(TiltType tiltType, double velU, double accU)
{
Result ret = Result.OK;
// 倾斜到位(胶阀当前倾斜状态不等于轨迹倾斜状态时执行)
if (tiltType != Machine.Instance.Valve1.CurTilt && (Machine.Instance.Robot.RobotIsXYZU || Machine.Instance.Robot.RobotIsXYZUV))
{
// 回到安全高度再变方向
if (Machine.Instance.Robot.PosZ < Machine.Instance.Robot.CalibPrm.SafeZ)
{
ret = Machine.Instance.Robot.MoveSafeZAndReply();
if (!ret.IsOk)
{
return(ret);
}
}
if (Machine.Instance.Robot.RobotIsXYZUV)
{
// todo 四方位旋转气缸动作
}
//U轴倾斜
AngleHeightPosOffset tiltValvePrm = Machine.Instance.Robot.CalibPrm.AngleHeightPosOffsetList.Find(t => t.TiltType.Equals(tiltType));
if (tiltValvePrm == null)
{
return(Result.FAILED);
}
if (velU == 0 || accU == 0)
{
return(Result.FAILED);
}
ret = Machine.Instance.Robot.MovePosUAndReply(tiltValvePrm.ValveAngle, velU, accU);
if (!ret.IsOk)
{
return(ret);
}
else
{
Machine.Instance.Valve1.CurTilt = tiltType;
}
}
return(ret);
}
