public void MoveTrc(MoveTrcPrm moveTrcPrm, IList <ICrdable> crds)
{
CommandMoveTrc command = new CommandMoveTrc(this.X, this.Y, moveTrcPrm, crds);
this.Fire(command);
}
public override Result FluidArc(PointD accStartPos, PointD arcStartPos, PointD arcEndPos, PointD decEndPos, PointD center, short clockwize, double vel, PointD[] points, double intervalSec, double acc)
{
if (Machine.Instance.Robot.IsSimulation)
{
return(Result.OK);
}
if (points == null)
{
return(Result.FAILED);
}
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet || Machine.Instance.Valve1.RunMode == ValveRunMode.Dry)
{
accStartPos = accStartPos.ToNeedle(this.valve1.Key);
arcStartPos = arcStartPos.ToNeedle(this.valve1.Key);
arcEndPos = arcEndPos.ToNeedle(this.valve1.Key);
decEndPos = decEndPos.ToNeedle(this.valve1.Key);
center = center.ToNeedle(this.valve1.Key);
for (int i = 0; i < points.Length; i++)
{
points[i] = points[i].ToNeedle(this.valve1.Key);
}
}
double interval = 0;
if (points.Length >= 2)
{
interval = points[0].DistanceTo(points[1]);
}
double accTime = this.CalcAccTime(accStartPos, arcStartPos, vel, acc * 1000);
//连续差补
List <ICrdable> crdList = new List <ICrdable>();
//加速段-直线
CrdLnXY crdAcc = new CrdLnXY()
{
EndPosX = arcStartPos.X,
EndPosY = arcStartPos.Y,
Vel = vel,
Acc = acc,
VelEnd = vel
};
crdList.Add(crdAcc);
//匀速段-圆
for (int i = 1; i < points.Length; i++)
{
CrdArcXYC crdArc = new CrdArcXYC()
{
EndPosX = points[i].X,
EndPosY = points[i].Y,
CenterX = center.X - points[i - 1].X,
CenterY = center.Y - points[i - 1].Y,
Clockwise = clockwize,
Vel = vel,
Acc = acc,
VelEnd = vel
};
crdList.Add(crdArc);
}
//减速段-直线
CrdLnXY crdDec = new CrdLnXY()
{
EndPosX = decEndPos.X,
EndPosY = decEndPos.Y,
Vel = vel,
Acc = acc,
VelEnd = 0
};
crdList.Add(crdDec);
Result rtn = Result.OK;
int intervalTime = (int)(intervalSec * 1000000);
Action timeCtrlAction = () =>
{
CommandMoveTrc command = new CommandMoveTrc(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrm,
crdList);
command.Starting += () =>
{
ValveSprayServer.Instance.StartSprayEvent.Set();
};
ValveSprayServer.Instance.SleepSpan = TimeSpan.FromSeconds(accTime);
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
ValveSprayServer.Instance.SprayAction = () =>
{
this.SprayCycle((short)points.Count(), (short)(intervalTime - this.jtValve1.Prm.OnTime));
};
}
else
{
ValveSprayServer.Instance.SprayAction = null;
}
Log.Dprint(string.Format("Fluid Line[time ctrl]-> accTime:{0}, intervalTime:{1}, onTime:{2}, offTime{3}.",
accTime, intervalTime, this.jtValve1.Prm.OnTime, this.jtValve1.Prm.OffTime));
Machine.Instance.Robot.Fire(command);
rtn = Machine.Instance.Robot.WaitCommandReply(command);
};
Action cmp2dAction = () =>
{
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
SimulCmp2dStart((short)Cmp2dSrcType.编码器, (short)this.jtValve1.Prm.Cmp2dMaxErr, points);
}
CommandMoveTrc command = new CommandMoveTrc(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrm,
crdList);
Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}, interval:{1}", this.jtValve1.Prm.Cmp2dMaxErr, interval));
Machine.Instance.Robot.Fire(command);
rtn = Machine.Instance.Robot.WaitCommandReply(command);
//if (this.RunMode == ValveRunMode.Wet)
//{
// Cmp2dStop();
//}
};
if (this.jtValve1.Prm.FluidMode == ValveFluidMode.时间控制优先)
{
if (intervalTime - this.jtValve1.Prm.OnTime < short.MaxValue)
{//判断脉宽是否short类型溢出，
timeCtrlAction();
}
else
{//溢出：位置比较模式
cmp2dAction();
}
}
else
{
if (interval * 1000 > this.jtValve1.Prm.Cmp2dMaxErr || points.Length < 2)
{
cmp2dAction();
}
else
{
timeCtrlAction();
}
}
return(rtn);
}
public override Result FluidSymbolLines(List <CrdSymbolLine> symbolLinesCrdData, GearValveFluidSymbolLinesPrm fluidPrm, double acc, double offsetX = 0, double offsetY = 0)
{
Result ret = Result.OK;
//如果不是CV模式，则要把坐标都转换到阀上
if (this.RunMode != ValveRunMode.Look)
{
PointD offset = new PointD(offsetX, offsetY);
foreach (var item in symbolLinesCrdData)
{
if (item.Type == 0)
{
item.Points[0] = item.Points[0].ToNeedle(this.ValveType) + offset;
item.Points[1] = item.Points[1].ToNeedle(this.ValveType) + offset;
}
else
{
item.Points[0] = item.Points[0].ToNeedle(this.ValveType) + offset;
item.Points[1] = item.Points[1].ToNeedle(this.ValveType) + offset;
item.Points[2] = item.Points[2].ToNeedle(this.ValveType) + offset;
}
}
}
//解析复合线段为对应的插补指令
List <ICrdable> crdList = new List <ICrdable>();
for (int i = 0; i < symbolLinesCrdData.Count; i++)
{
//如果是直线，则先要判断高度是否一致，一致的话添加一个两轴直线插补，不一致的话要先添加一个BufGear插补
if (symbolLinesCrdData[i].Type == 0)
{
if (Machine.Instance.Robot.AxisZ.Pos != symbolLinesCrdData[i].EndZ)
{
CrdXYGear crdXYGear = new CrdXYGear()
{
GearAxis = Machine.Instance.Robot.AxisZ,
DeltaPulse = Machine.Instance.Robot.AxisZ.ConvertPos2Card(symbolLinesCrdData[i].EndZ)
};
crdList.Add(crdXYGear);
}
CrdLnXY crd = new CrdLnXY()
{
EndPosX = symbolLinesCrdData[i].Points[1].X,
EndPosY = symbolLinesCrdData[i].Points[1].Y,
Vel = fluidPrm.Vel,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = (i != symbolLinesCrdData.Count - 1 ? 0 : fluidPrm.Vel),
};
crdList.Add(crd);
}
//如果是圆弧，则要先添加一个BufGear插补，再添加一个圆弧插补
else if (symbolLinesCrdData[i].Type == 1)
{
CrdXYGear crdXYGear = new CrdXYGear()
{
GearAxis = Machine.Instance.Robot.AxisR,
DeltaPulse = Machine.Instance.Robot.AxisR.ConvertPos2Card(symbolLinesCrdData[i].TrackSweep)
};
crdList.Add(crdXYGear);
double r = Math.Sqrt(Math.Pow(symbolLinesCrdData[i].Points[0].X - symbolLinesCrdData[i].Points[1].X, 2)
+ Math.Pow(symbolLinesCrdData[i].Points[0].Y - symbolLinesCrdData[i].Points[1].Y, 2));
CrdArcXYR crd = new CrdArcXYR
{
EndPosX = symbolLinesCrdData[i].Points[2].X,
EndPosY = symbolLinesCrdData[i].Points[2].Y,
R = r,
Clockwise = (short)symbolLinesCrdData[i].Clockwise,
Vel = fluidPrm.ArcSpeed,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = (i != symbolLinesCrdData.Count - 1 ? 0 : fluidPrm.Vel),
};
if (Math.Abs(symbolLinesCrdData[i].EndAngle) > 180)
{
crd.R = crd.R * -1;
}
crdList.Add(crd);
}
}
//计算关胶位置
PointD stopSprayPos = this.CalculateStopSprayPos(symbolLinesCrdData, fluidPrm.StopSprayDistance);
//终点位置
PointD endPos = new PointD();
//如果是Wet模式
if (this.RunMode == ValveRunMode.Wet)
{
//得到终点位置
if (symbolLinesCrdData.Last().Type == 0)
{
endPos = symbolLinesCrdData.Last().Points[1];
}
else
{
endPos = symbolLinesCrdData.Last().Points[2];
}
endPos.X = Math.Round(endPos.X, 3);
endPos.Y = Math.Round(endPos.Y, 3);
//如果线的终点和关胶点不一致，则可以启用二维比较
if (endPos != stopSprayPos)
{
this.CmpStop();
this.Cmp2dStart(stopSprayPos);
}
else
{
this.Spraying();
}
}
//移动前的出胶延时
Thread.Sleep(TimeSpan.FromSeconds(fluidPrm.StartSprayDelay));
//开始移动
CommandMoveTrc command = new CommandMoveTrc(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrm,
crdList)
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}", this.Prm.Cmp2dMaxErr));
Machine.Instance.Robot.Fire(command);
ret = Machine.Instance.Robot.WaitCommandReply(command);
if (!ret.IsOk)
{
return(ret);
}
if (this.RunMode == ValveRunMode.Wet)
{
//如果提前关胶距离是0，就直接关胶
if (fluidPrm.StopSprayDistance == 0)
{
this.SprayOff();
}
}
//在终点延时
Thread.Sleep(TimeSpan.FromSeconds(fluidPrm.EndPosDelay));
//if (this.RunMode != ValveRunMode.Look)
//{
// //下压
// ret = Machine.Instance.Robot.MoveIncZAndReply(-fluidPrm.PressDistance, fluidPrm.PressVel, fluidPrm.PressAcc);
// if (!ret.IsOk)
// return ret;
// //下压保持时间
// Thread.Sleep(TimeSpan.FromSeconds(fluidPrm.PressTime));
// //抬起
// ret = Machine.Instance.Robot.MoveIncZAndReply(fluidPrm.RaiseDistance, fluidPrm.RaiseVel, fluidPrm.RaiseAcc);
//}
//开始回走
//如果是Wet模式或者Dry模式
if (fluidPrm.BacktrackDistance == 0 || fluidPrm.BacktrackDistance.Equals(0))
{
}
else
{
if (this.RunMode == ValveRunMode.Wet || this.RunMode == ValveRunMode.Dry)
{
//回抬
ret = Machine.Instance.Robot.MoveIncZAndReply(fluidPrm.BacktrackGap, fluidPrm.BacktrackSpeed, acc);
//计算回走终点位置
PointD backTrackPoint = CalculateBackTrackPos(symbolLinesCrdData, fluidPrm.BacktrackDistance);
//执行回走
List <ICrdable> crdXYZList = new List <ICrdable>();
if (symbolLinesCrdData.Last().Type == 0) //直线
{
CrdXYGear crdXYGear = new CrdXYGear()
{
GearAxis = Machine.Instance.Robot.AxisZ,
DeltaPulse = Machine.Instance.Robot.AxisZ.ConvertPos2Card(fluidPrm.BacktrackEndGap - fluidPrm.BacktrackGap)
};
crdXYZList.Add(crdXYGear);
CrdLnXY crd = new CrdLnXY()
{
EndPosX = backTrackPoint.X,
EndPosY = backTrackPoint.Y,
Vel = fluidPrm.BacktrackSpeed,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = 0,
};
crdXYZList.Add(crd);
}
else //圆弧
{
CrdXYGear crdXYZGear = new CrdXYGear()
{
GearAxis = Machine.Instance.Robot.AxisZ,
DeltaPulse = Machine.Instance.Robot.AxisZ.ConvertPos2Card(fluidPrm.BacktrackEndGap - fluidPrm.BacktrackGap)
};
crdXYZList.Add(crdXYZGear);
CrdXYGear crdXYRGear = new CrdXYGear()
{
GearAxis = Machine.Instance.Robot.AxisR,
DeltaPulse = Machine.Instance.Robot.AxisR.ConvertPos2Card(-symbolLinesCrdData.Last().TrackSweep *fluidPrm.BacktrackDistance / 100)
};
crdXYZList.Add(crdXYRGear);
double r = Math.Sqrt(Math.Pow(symbolLinesCrdData.Last().Points[0].X - symbolLinesCrdData.Last().Points[1].X, 2)
+ Math.Pow(symbolLinesCrdData.Last().Points[0].Y - symbolLinesCrdData.Last().Points[1].Y, 2));
CrdArcXYR crd = new CrdArcXYR
{
EndPosX = backTrackPoint.X,
EndPosY = backTrackPoint.Y,
R = r,
Clockwise = (short)symbolLinesCrdData.Last().Clockwise == (short)0 ? (short)1 : (short)0,
Vel = fluidPrm.BacktrackSpeed,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = 0,
};
if (Math.Abs(symbolLinesCrdData.Last().EndAngle *fluidPrm.BacktrackDistance / 100) > 180)
{
crd.R = crd.R * -1;
}
crdXYZList.Add(crd);
}
CommandMoveTrc command2 = new CommandMoveTrc(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrm,
crdXYZList)
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
Machine.Instance.Robot.Fire(command2);
ret = Machine.Instance.Robot.WaitCommandReply(command2);
}
}
//结束回走
if (this.RunMode == ValveRunMode.Wet || this.RunMode == ValveRunMode.Dry)
{
//抬起到回位高度
ret = Machine.Instance.Robot.MoveIncZAndReply(fluidPrm.BackGap, fluidPrm.Vel, acc);
}
return(ret);
}
public override Result FluidLine(PointD accStartPos, PointD lineStartPos, PointD lineEndPos, PointD decEndPos, double vel, PointD[] points, double intervalSec, double acc)
{
if (Machine.Instance.Robot.IsSimulation)
{
return(Result.OK);
}
if (points == null)
{
return(Result.FAILED);
}
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet || Machine.Instance.Valve1.RunMode == ValveRunMode.Dry)
{
accStartPos = accStartPos.ToNeedle(this.valve1.Key);
lineStartPos = lineStartPos.ToNeedle(this.valve1.Key);
lineEndPos = lineEndPos.ToNeedle(this.valve1.Key);
decEndPos = decEndPos.ToNeedle(this.valve1.Key);
for (int i = 0; i < points.Length; i++)
{
points[i] = points[i].ToNeedle(this.valve1.Key);
}
}
double interval = 0;
if (points.Length >= 2)
{
interval = points[0].DistanceTo(points[1]);
}
//计算加速段时间，用于启动时间控制打胶
double accTime = this.CalcAccTime(accStartPos, lineStartPos, vel, acc * 1000);
//连续插补
List <ICrdable> crdList = new List <ICrdable>();
//加速段
CrdLnXY crdAcc = new CrdLnXY()
{
EndPosX = lineStartPos.X,
EndPosY = lineStartPos.Y,
Vel = vel,
Acc = acc,
VelEnd = vel
};
crdList.Add(crdAcc);
//匀速段
for (int i = 1; i < points.Length; i++)
{
CrdLnXY crdLn = new CrdLnXY()
{
EndPosX = points[i].X,
EndPosY = points[i].Y,
Vel = vel,
Acc = acc,
VelEnd = vel
};
crdList.Add(crdLn);
}
//减速段
CrdLnXY crdDec = new CrdLnXY()
{
EndPosX = decEndPos.X,
EndPosY = decEndPos.Y,
Vel = vel,
Acc = acc,
VelEnd = 0
};
crdList.Add(crdDec);
DateTime t1 = DateTime.Now;
DateTime t2 = DateTime.Now;
Result rtn = Result.OK;
int intervalTime = (int)(intervalSec * 1000000);
//时间控制函数
Action timeCtrlAction = () =>
{
ICommandable command = null;
if (this.jtValve1.Prm.MoveMode == ValveMoveMode.单次插补)
{//单次插补
CrdLnXY crd = new CrdLnXY()
{
EndPosX = decEndPos.X,
EndPosY = decEndPos.Y,
Vel = vel,
Acc = acc,
VelEnd = vel
};
command = new CommandMoveTrc(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrm,
crd,
() =>
{
t1 = DateTime.Now;
ValveSprayServer.Instance.StartSprayEvent.Set();
})
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
}
else
{//多次插补
command = new CommandMoveTrc(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrm,
crdList,
() =>
{
t1 = DateTime.Now;
ValveSprayServer.Instance.StartSprayEvent.Set();
})
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
}
//设置打胶线程启动时间
ValveSprayServer.Instance.SleepSpan = TimeSpan.FromSeconds(accTime);
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
ValveSprayServer.Instance.SprayAction = () =>
{
t2 = DateTime.Now;
this.SprayCycle((short)points.Count(), (short)(intervalTime - this.jtValve1.Prm.OnTime));
};
}
else
{
ValveSprayServer.Instance.SprayAction = null;
}
Log.Dprint(string.Format("Fluid Line[time ctrl]-> accTime:{0}, intervalTime:{1}, onTime:{2}, offTime{3}.",
accTime, intervalTime, this.jtValve1.Prm.OnTime, this.jtValve1.Prm.OffTime));
Machine.Instance.Robot.Fire(command);
rtn = Machine.Instance.Robot.WaitCommandReply(command);
Debug.WriteLine(t1.ToString("HH::mm::ss::fff"));
Debug.WriteLine(t2.ToString("HH::mm::ss::fff"));
};
//2d比较控制函数
Action cmp2dAction = () =>
{
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
CmpStop();
SimulCmp2dStart((short)Cmp2dSrcType.编码器, (short)this.jtValve1.Prm.Cmp2dMaxErr, points);
}
if (this.jtValve1.Prm.MoveMode == ValveMoveMode.单次插补)
{
rtn = Machine.Instance.Robot.MovePosXYAndReply(decEndPos, vel, acc);
}
else
{
CommandMoveTrc command = new CommandMoveTrc(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrm,
crdList)
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}, interval:{1}", this.jtValve1.Prm.Cmp2dMaxErr, interval));
Machine.Instance.Robot.Fire(command);
rtn = Machine.Instance.Robot.WaitCommandReply(command);
}
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
//Cmp2dStop();
}
};
//一维比较控制函数
Action cmp1dAction = () =>
{
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
SimulCmp2dStop();
CmpStart((short)Cmp2dSrcType.编码器, points);
}
if (this.jtValve1.Prm.MoveMode == ValveMoveMode.单次插补)
{
rtn = Machine.Instance.Robot.MovePosXYAndReply(decEndPos, vel, acc);
}
else
{
CommandMoveTrc command = new CommandMoveTrc(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrm,
crdList)
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}, interval:{1}", this.jtValve1.Prm.Cmp2dMaxErr, interval));
Machine.Instance.Robot.Fire(command);
rtn = Machine.Instance.Robot.WaitCommandReply(command);
}
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
//CmpStop();
}
};
switch (this.jtValve1.Prm.FluidMode)
{
case ValveFluidMode.一维比较优先:
cmp1dAction();
break;
case ValveFluidMode.二维比较优先:
if (interval * 1000 > this.jtValve1.Prm.Cmp2dMaxErr || points.Length < 2)
{
cmp2dAction();
}
else
{
//timeCtrlAction();
cmp1dAction();
}
break;
case ValveFluidMode.时间控制优先:
if (intervalTime - this.jtValve1.Prm.OnTime < short.MaxValue)
{ //判断脉宽是否short类型溢出，
timeCtrlAction();
}
else
{ //溢出：位置比较模式
//cmp2dAction();
cmp1dAction();
}
break;
}
return(rtn);
}
