/// <summary>
/// 螺杆阀CV模式模式下的运行逻辑
/// </summary>
/// <returns></returns>
private Result SvValveCVLogic(MultiTraces multiTraces)
{
Result ret = Result.OK;
ret = Machine.Instance.Robot.MoveSafeZAndReply();
if (!ret.IsOk)
{
return(ret);
}
if (multiTraces.Traces.Count == 0)
{
return(ret);
}
// 移动到轨迹起点
ret = Machine.Instance.Robot.MovePosXYAndReply(multiTraces.Traces[0].Start,
FluidProgram.Current.MotionSettings.VelXY,
FluidProgram.Current.MotionSettings.AccXY);
if (!ret.IsOk)
{
return(ret);
}
SvValveFludLineParam primaryLineParam = this.GetSvValveParam(multiTraces, multiTraces.Traces);
// 连续插补
double[] vels = new double[multiTraces.Traces.Count];
for (int i = 0; i < vels.Length; i++)
{
vels[i] = this.getLineParam(multiTraces, multiTraces.Traces[i]).Speed;
}
List <ICrdable> crdList = Valve.GetCrdsBy(multiTraces.Traces, vels, FluidProgram.Current.MotionSettings.AccXY);
ret = Machine.Instance.Robot.MoveTrcXYReply(crdList);
if (!ret.IsOk)
{
return(ret);
}
List <ICrdable> backCrdList = Valve.GetCrdsBy(primaryLineParam.backTransTraces, primaryLineParam.backTrackVel, FluidProgram.Current.MotionSettings.AccXY);
return(Machine.Instance.Robot.MoveTrcXYReply(backCrdList));
}
