private short AddCrdLnXY(CrdLnXY crd)
{
return(this.card.Executor.AddCrdLnXY(
this.card.CardId,
this.TrcPrm.CsId,
this.Axes[0].ConvertPos2Card(crd.EndPosX),
this.Axes[1].ConvertPos2Card(crd.EndPosY),
this.Axes[0].ConvertVel2Card(crd.Vel),
crd.Acc,
this.Axes[0].ConvertVel2Card(crd.VelEnd)));
}
private short AddCrd2Space()
{
short rtn = 0;
this.card.Executor.GetCrdSpace(this.card.CardId, this.TrcPrm.CsId, out space);
int count = (space < MAX_ONCE) ? space : MAX_ONCE;
for (int i = 0; i < count; i++)
{
if (this.crdQueue.Count > 0)
{
ICrdable crd = this.crdQueue.Dequeue();
switch (crd.Type)
{
case CrdType.ArcXYC:
CrdArcXYC crdArcXYC = crd as CrdArcXYC;
rtn = this.AddCrdArcXYC(crdArcXYC);
break;
case CrdType.ArcXYR:
CrdArcXYR crdArcXYR = crd as CrdArcXYR;
rtn = this.AddCrdArcXYR(crdArcXYR);
break;
case CrdType.BufIO:
rtn = this.AddCrdBufIO(crd as CrdBufIO);
break;
case CrdType.Delay:
rtn = this.AddCrdDelay(crd as CrdDelay);
break;
case CrdType.LnXY:
CrdLnXY crdLnXY = crd as CrdLnXY;
rtn = this.AddCrdLnXY(crdLnXY);
break;
case CrdType.XYGear:
CrdXYGear crdXYGear = crd as CrdXYGear;
rtn = this.AddXYGear(crdXYGear);
break;
case CrdType.BufMove:
rtn = this.AddBufMove(crd as CrdBufMove);
break;
}
}
else
{
break;
}
}
return(rtn);
}
private void AddCrd2Space()
{
this.card.Executor.GetCrdSpace(this.card.CardId, this.TrcPrm.CsId, out space);
int count = (space < MAX_ONCE) ? space : MAX_ONCE;
for (int i = 0; i < count; i++)
{
if (this.crdQueue.Count > 0)
{
ICrdable crd = this.crdQueue.Dequeue();
switch (crd.Type)
{
case CrdType.ArcXYC:
CrdArcXYC crdArcXYC = crd as CrdArcXYC;
this.AddCrdArcXYC(crdArcXYC);
break;
case CrdType.ArcXYR:
CrdArcXYR crdArcXYR = crd as CrdArcXYR;
this.AddCrdArcXYR(crdArcXYR);
break;
case CrdType.BufIO:
this.AddCrdBufIO(crd as CrdBufIO);
break;
case CrdType.Delay:
this.AddCrdDelay(crd as CrdDelay);
break;
case CrdType.LnXY:
CrdLnXY crdLnXY = crd as CrdLnXY;
this.AddCrdLnXY(crdLnXY);
break;
case CrdType.LnXYR:
CrdLnXYR crdLnXYA = crd as CrdLnXYR;
this.AddCrdLnXYA(crdLnXYA);
break;
}
}
else
{
break;
}
}
}
private void AddCrd2Space()
{
this.card.Executor.GetCrdSpace(this.card.CardId, this.TrcPrm.CsId, out space);
int count = (space < MAX_ONCE) ? space : MAX_ONCE;
for (int i = 0; i < count; i++)
{
if (this.crdQueue.Count > 0)
{
ICrdable crd = this.crdQueue.Dequeue();
switch (crd.Type)
{
case CrdType.ArcXYC:
CrdArcXYC crdArcXYC = crd as CrdArcXYC;
this.AddCrdArcXYC(crdArcXYC);
break;
case CrdType.ArcXYR:
CrdArcXYR crdArcXYR = crd as CrdArcXYR;
this.AddCrdArcXYR(crdArcXYR);
break;
case CrdType.BufIO:
this.AddCrdBufIO(crd as CrdBufIO);
break;
case CrdType.Delay:
this.AddCrdDelay(crd as CrdDelay);
break;
case CrdType.LnXY:
CrdLnXY crdLnXY = crd as CrdLnXY;
this.AddCrdLnXY(crdLnXY);
break;
}
}
else
{
//没有插补数据了一次性将前瞻缓存区数据塞到插补缓存区
this.card.Executor.GT_CrdData(this.card.CardId, this.InitLoodPrm.crd, System.IntPtr.Zero, this.InitLoodPrm.fifo);
break;
}
}
}
private short addCrd(ICrdable crd)
{
short rtn = 0;
switch (crd.Type)
{
case CrdType.ArcXYC:
CrdArcXYC crdArcXYC = crd as CrdArcXYC;
rtn = this.addCrdArcXYC(crdArcXYC);
break;
case CrdType.ArcXYR:
CrdArcXYR crdArcXYR = crd as CrdArcXYR;
rtn = this.addCrdArcXYR(crdArcXYR);
break;
case CrdType.BufIO:
rtn = this.addCrdBufIO(crd as CrdBufIO);
break;
case CrdType.Delay:
rtn = this.addCrdDelay(crd as CrdDelay);
break;
case CrdType.LnXY:
CrdLnXY crdLnXY = crd as CrdLnXY;
rtn = this.addCrdLnXY(crdLnXY);
break;
case CrdType.XYGear:
CrdXYGear crdXYGear = crd as CrdXYGear;
rtn = this.addXYGear(crdXYGear);
break;
case CrdType.BufMove:
rtn = this.addBufMove(crd as CrdBufMove);
break;
}
return(rtn);
}
/// <summary>
/// 根据轨迹集合生成插补列表
/// </summary>
/// <param name="traces"></param>
/// <param name="vels"></param>
/// <param name="acc"></param>
/// <returns></returns>
public static List <ICrdable> GetCrdsBy(List <TraceBase> traces, double[] vels, double acc)
{
List <ICrdable> crdList = new List <ICrdable>();
for (int i = 0; i < traces.Count; i++)
{
TraceBase trace = traces[i];
if (trace is TraceLine)
{
CrdLnXY crd = new CrdLnXY()
{
EndPosX = trace.End.X,
EndPosY = trace.End.Y,
Vel = vels[i],
Acc = acc,
VelEnd = i == (traces.Count - 1) ? 0 : vels[i + 1]
};
crdList.Add(crd);
}
else if (trace is TraceArc)
{
CrdArcXYC crd = new CrdArcXYC()
{
EndPosX = trace.End.X,
EndPosY = trace.End.Y,
CenterX = (trace as TraceArc).Center.X - trace.Start.X,
CenterY = (trace as TraceArc).Center.Y - trace.Start.Y,
Clockwise = (short)((trace as TraceArc).Degree > 0 ? 1 : 0),
Vel = vels[i],
Acc = acc,
VelEnd = i == (traces.Count - 1) ? 0 : vels[i + 1]
};
crdList.Add(crd);
}
}
return(crdList);
}
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 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 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);
}
public override Result FluidLine(SvValveFludLineParam svValveLineParam, double acc)
{
Result ret = Result.OK;
if (Machine.Instance.Robot.IsSimulation)
{
return(Result.OK);
}
if (svValveLineParam.endPos == null)
{
return(Result.FAILED);
}
//TODO 前瞻预处理还没加
//连续插补
List <ICrdable> crdList = new List <ICrdable>();
if (!svValveLineParam.IsTraceMode)
{
// 多段线模式
if (svValveLineParam.transPoints.Length > 0)
{
for (int i = 0; i < svValveLineParam.transPoints.Length; i++)
{
CrdLnXY crd = new CrdLnXY()
{
EndPosX = svValveLineParam.transPoints[i].X,
EndPosY = svValveLineParam.transPoints[i].Y,
Vel = svValveLineParam.vels[i],
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = svValveLineParam.vels[i]
};
crdList.Add(crd);
}
}
}
else
{
// 线+圆弧轨迹模式
crdList = GetCrdsBy(svValveLineParam.transTraces, svValveLineParam.vels, acc);
}
if (this.RunMode == ValveRunMode.Wet)
{
//如果线的终点和关胶点不一致，则可以启用二维比较
if (svValveLineParam.endPos != svValveLineParam.stopSprayPos)
{
this.CmpStop();
this.Cmp2dStart(svValveLineParam.stopSprayPos);
}
else
{
this.Spraying();
}
}
//在起始位置开胶后，延时移动的时间
Thread.Sleep(TimeSpan.FromSeconds(svValveLineParam.startPosDelay));
//开始移动
if (!svValveLineParam.IsTraceMode)
{
if (svValveLineParam.transPoints.Length > 0)
{
CommandMoveTrcCmp2d command = new CommandMoveTrcCmp2d(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrmWeight,
crdList, this.Chn, svValveLineParam.transPoints.ToList())
{
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);
}
else
{
ret = Machine.Instance.Robot.MovePosXYAndReply(svValveLineParam.endPos, svValveLineParam.vels[0], acc);
}
}
else
{
ret = Machine.Instance.Robot.MoveTrcXYReply(crdList);
}
//如果提前关胶距离是0，就手动关胶
if (svValveLineParam.endPos == svValveLineParam.stopSprayPos)
{
this.SprayOff();
}
//在终点延时
Thread.Sleep(TimeSpan.FromSeconds(svValveLineParam.backTrackDelay));
//抬高
ret = Machine.Instance.Robot.MoveIncZAndReply(svValveLineParam.backTrackGap, svValveLineParam.backTrackVel, acc);
if (!ret.IsOk)
{
return(ret);
}
//开始回走
//连续插补
List <ICrdable> backCrdList = new List <ICrdable>();
if (!svValveLineParam.IsTraceMode)
{
if (svValveLineParam.backTransPoints.Length > 0)
{
for (int i = 0; i < svValveLineParam.backTransPoints.Length; i++)
{
CrdLnXY crd = new CrdLnXY()
{
EndPosX = svValveLineParam.backTransPoints[i].X,
EndPosY = svValveLineParam.backTransPoints[i].Y,
Vel = svValveLineParam.backTrackVel,
Acc = acc,
VelEnd = svValveLineParam.backTrackVel
};
backCrdList.Add(crd);
}
}
}
else
{
// 线+圆弧轨迹模式
backCrdList = GetCrdsBy(svValveLineParam.backTransTraces, svValveLineParam.backTrackVel, acc);
}
if (!svValveLineParam.IsTraceMode)
{
if (svValveLineParam.backTransPoints.Length > 0)
{
CommandMoveTrcCmp2d command = new CommandMoveTrcCmp2d(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrmWeight,
backCrdList, this.Chn, svValveLineParam.backTransPoints.ToList())
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
Machine.Instance.Robot.Fire(command);
ret = Machine.Instance.Robot.WaitCommandReply(command);
}
else
{
ret = Machine.Instance.Robot.MovePosXYAndReply(svValveLineParam.backTrackPos, svValveLineParam.vels[0], acc);
}
}
else
{
ret = Machine.Instance.Robot.MoveTrcXYReply(backCrdList);
}
return(ret);
}