private async Task incMove(IMove iMove, double speedSP)
{
if (isSafeToMove(iMove))
{
short ret = 0;
StringBuilder vType = new StringBuilder("V"); // VR;
double speed = (speedSP > Limits.maxLinearSpeed) ? Limits.maxLinearSpeed : ((speedSP < 0) ? 0 : (double)speedSP); // is VJ=20.00
StringBuilder framename = new StringBuilder("TOOL"); // ROBOT BASE
short toolNo = 0;
double[] iCoordinates = iMove.ToArray();
if (await IsReadyToOperate())
{
await Task.Delay(commsDelay_mm);
MovingToPosition?.Invoke(this, null);
ret = CMotocom.BscImov(m_Handle, vType, speed, framename, toolNo, ref iCoordinates[0]);
if (ret != 0)
{
SetError("Error MoveToPosition:BscImov");
}
}
}
else
{
SetError("Not safe to move to X=" + movingTo[0] + ", Y=" + movingTo[1] + ", Z=" + movingTo[2] + ", Rz=" + movingTo[5]);
}
}
public async Task MoveToHome1(double speedSP = 20)
{
if (m_commsError)
{
return;
}
await TaskEx.WaitUntil(isIdle);
SetEvent("MoveToHome1");
short ret = 0;
double speed = (speedSP > Limits.maxJointSpeed) ? Limits.maxJointSpeed : ((speedSP < 0) ? 0 : speedSP);
short toolNo = 0;
if (await IsReadyToOperate())
{
if (currentPosition[2] < homePosXYZ[2] - 500)
{
IMove iMove = new IMove(0, 0, -300, 0);
await incMove(iMove, Limits.maxLinearSpeed);
}
movingTo[0] = homePosXYZ[0];
movingTo[1] = homePosXYZ[1];
movingTo[2] = homePosXYZ[2];
movingTo[3] = homePosXYZ[3];
movingTo[4] = homePosXYZ[4];
movingTo[5] = homePosXYZ[5];
await Task.Delay(commsDelay_mm);
MovingToPosition?.Invoke(this, null);
ret = CMotocom.BscPMovj(m_Handle, speed, toolNo, ref homePosPulse[0]);
if (ret != 0)
{
SetError("Error MoveToHome1:BscPMovj");
}
}
await SetEvent(EnumEvent.Idle);
}
