public void Glue(GlueTargets glueTargets, GlueParameter gluePara)
{
lock (_motionLocker)
{
ClearInterpolationBuffer();
PointInfo gluePoint;
PointInfo risePoint;
for (int i = 0; i < 4; i++)
{
gluePoint = new PointInfo()
{
X = glueTargets.PointTargets[i].X,
Y = glueTargets.PointTargets[i].Y,
Z = glueTargets.PointTargets[i].Z + gluePara.GlueRadius,
};
AddPoint(gluePoint);
AddDigitalOutput(OutputState.On);
if (i == 0)
{
AddDelay(gluePara.PreShotTime);
}
else
{
AddDelay(gluePara.PreShotTime - gluePara.SecondLineLessPreShot);
}
AddDelay(gluePara.GluePeriod);
AddDigitalOutput(OutputState.Off);
risePoint = new PointInfo()
{
X = glueTargets.PointTargets[i].X,
Y = glueTargets.PointTargets[i].Y,
Z = glueTargets.PointTargets[i].Z + gluePara.RiseGlueHeight,
};
AddPoint(risePoint, gluePara.RiseGlueSpeed);
AddDelay(gluePara.CloseGlueDelay);
}
var lastRise = new PointInfo()
{
X = glueTargets.PointTargets[3].X,
Y = glueTargets.PointTargets[3].Y,
Z = glueTargets.PointTargets[3].Z + 20,
};
AddPoint(lastRise);
CheckEnoughSpace();
StartInterpolation();
WaitTillInterpolationEnd();
}
}
public PointInfoPulse ConvertToPulse(PointInfo point)
{
return(new PointInfoPulse()
{
X = (int)(MotorX.EncoderFactor * point.X * MotorX.Direction),
Y = (int)(MotorY.EncoderFactor * point.Y * MotorY.Direction),
Z = (int)(MotorY.EncoderFactor * point.Z * MotorZ.Direction),
Velocity = MotorX.Velocity,
Acceleration = MotorX.Acceleration,
});
}
public GlueTargets GetVisionResultsForLaserAndWork()
{
var id = CaptureId.GluePointBeforeGlue;
var offset = GetRawVisionResult(GetCapturePosition(id), 3);
#region Laser targets.
var laserTargets = new Pose[4];
int indexL = 0;
laserTargets[indexL] = new Pose()
{
X = offset.LaserX[indexL],
Y = offset.LaserY[indexL],
Z = GetZHeight(id),
};
indexL++;
laserTargets[indexL] = new Pose()
{
X = offset.LaserX[indexL],
Y = offset.LaserY[indexL],
Z = GetZHeight(id),
};
indexL++;
laserTargets[indexL] = new Pose()
{
X = offset.LaserX[indexL],
Y = offset.LaserY[indexL],
Z = GetZHeight(id),
};
indexL++;
laserTargets[indexL] = new Pose()
{
X = offset.LaserX[indexL],
Y = offset.LaserY[indexL],
Z = GetZHeight(id),
};
#endregion
#region point infos
var glueTargets = new PointInfo[4];
int indexG = 0;
glueTargets[indexG] = new PointInfo()
{
X = offset.StartPointX[indexG],
Y = offset.StartPointY[indexG],
};
indexG++;
glueTargets[indexG] = new PointInfo()
{
X = offset.StartPointX[indexG],
Y = offset.StartPointY[indexG],
};
indexG++;
glueTargets[indexG] = new PointInfo()
{
X = offset.StartPointX[indexG],
Y = offset.StartPointY[indexG],
};
indexG++;
glueTargets[indexG] = new PointInfo()
{
X = offset.StartPointX[indexG],
Y = offset.StartPointY[indexG],
};
#endregion
return(new GlueTargets()
{
LaserTargets = laserTargets,
PointTargets = glueTargets,
});
}
public void GlueOld(GlueTargets glueTargets, GlueParameter gluePara)
{
ClearInterpolationBuffer();
#region First point.
//Go to first point and delay.
int index = 0;
var firstPoint = new PointInfo()
{
X = glueTargets.ArcTargets[index].XStart,
Y = glueTargets.ArcTargets[index].YStart,
Z = glueTargets.ArcTargets[index].Z + gluePara.GlueHeightOffset[index],
};
AddPoint(firstPoint);
AddDigitalOutput(OutputState.On);
AddDelay(gluePara.PreShotTime);
AddArc(glueTargets.ArcTargets[index], gluePara.GlueSpeed);
//Todo Pre close to avoid sharp ending.
AddDigitalOutput(OutputState.Off);
var firstRise = new PointInfo()
{
X = glueTargets.ArcTargets[index].XEnd,
Y = glueTargets.ArcTargets[index].YEnd,
Z = glueTargets.ArcTargets[index].Z + gluePara.RiseGlueHeight,
};
AddPoint(firstRise, gluePara.RiseGlueSpeed);
AddDelay(gluePara.CloseGlueDelay);
#endregion
#region Second point.
index++;
var secondPoint = new PointInfo()
{
X = glueTargets.ArcTargets[index].XStart,
Y = glueTargets.ArcTargets[index].YStart,
Z = glueTargets.ArcTargets[index].Z + gluePara.GlueHeightOffset[index],
};
AddPoint(secondPoint);
AddDigitalOutput(OutputState.On);
AddDelay(gluePara.PreShotTime - gluePara.SecondLineLessPreShot);
AddArc(glueTargets.ArcTargets[index], gluePara.GlueSpeed);
//Todo Pre close to avoid sharp ending.
AddDigitalOutput(OutputState.Off);
var secondRise = new PointInfo()
{
X = glueTargets.ArcTargets[index].XEnd,
Y = glueTargets.ArcTargets[index].YEnd,
Z = glueTargets.ArcTargets[index].Z + gluePara.RiseGlueHeight,
};
AddPoint(secondRise, gluePara.RiseGlueSpeed);
AddDelay(gluePara.CloseGlueDelay);
#endregion
#region third point.
index++;
var thirdPoint = new PointInfo()
{
X = glueTargets.ArcTargets[index].XStart,
Y = glueTargets.ArcTargets[index].YStart,
Z = glueTargets.ArcTargets[index].Z + gluePara.GlueHeightOffset[index],
};
AddPoint(thirdPoint);
AddDigitalOutput(OutputState.On);
AddDelay(gluePara.PreShotTime - gluePara.SecondLineLessPreShot);
AddArc(glueTargets.ArcTargets[index], gluePara.GlueSpeed);
//Todo Pre close to avoid sharp ending.
AddDigitalOutput(OutputState.Off);
var thirdRise = new PointInfo()
{
X = glueTargets.ArcTargets[index].XEnd,
Y = glueTargets.ArcTargets[index].YEnd,
Z = glueTargets.ArcTargets[index].Z + gluePara.RiseGlueHeight,
};
AddPoint(thirdRise, gluePara.RiseGlueSpeed);
AddDelay(gluePara.CloseGlueDelay);
#endregion
#region fourth point.
index++;
var fourthPoint = new PointInfo()
{
X = glueTargets.ArcTargets[index].XStart,
Y = glueTargets.ArcTargets[index].YStart,
Z = glueTargets.ArcTargets[index].Z + gluePara.GlueHeightOffset[index],
};
AddPoint(fourthPoint);
AddDigitalOutput(OutputState.On);
AddDelay(gluePara.PreShotTime - gluePara.SecondLineLessPreShot);
AddArc(glueTargets.ArcTargets[index], gluePara.GlueSpeed);
//Todo Pre close to avoid sharp ending.
AddDigitalOutput(OutputState.Off);
var fourthRise = new PointInfo()
{
X = glueTargets.ArcTargets[index].XEnd,
Y = glueTargets.ArcTargets[index].YEnd,
Z = glueTargets.ArcTargets[index].Z + gluePara.RiseGlueHeight,
};
AddPoint(fourthRise, gluePara.RiseGlueSpeed);
AddDelay(gluePara.CloseGlueDelay);
#endregion
var finishRise = new PointInfo()
{
X = glueTargets.ArcTargets[index].XEnd,
Y = glueTargets.ArcTargets[index].YEnd,
Z = glueTargets.ArcTargets[index].Z + gluePara.RiseGlueHeight + 20,
};
AddPoint(finishRise, 10);
CheckEnoughSpace();
StartInterpolation();
WaitTillInterpolationEnd();
}
