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 short AddCrdArcXYR(CrdArcXYR crd)
{
return(this.card.Executor.AddCrdArcXYR(
this.card.CardId,
this.TrcPrm.CsId,
this.Axes[0].ConvertPos2Card(crd.EndPosX),
this.Axes[1].ConvertPos2Card(crd.EndPosY),
this.Axes[0].ConvertPos2Card(crd.R),
crd.Clockwise,
this.Axes[0].ConvertVel2Card(crd.Vel),
crd.Acc,
this.Axes[0].ConvertVel2Card(crd.VelEnd)));
}
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);
}
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);
}
