public void DirectDo(string command, TsPointS stopPoint, bool isWait)
{
_Robot.DirectDo(command);
if (isWait == false || stopPoint == null)
{
return;
}
Thread.Sleep(100);
TsPointS point_bak = null;
int MoveStopCnt = 0;
double bakAngle = 999;
while ((ConnectStatus & CONNECTTYPE.SERVOON) > 0)
{
TsPointS point = _Robot.GetPsnFbkWork();
if ((Math.Abs(point.X - stopPoint.X) < 0.01) &&
(Math.Abs(point.Y - stopPoint.Y) < 0.01) &&
(Math.Abs(point.Z - stopPoint.Z) < 0.01) /*&&
* (Math.Abs(point.C - stopPoint.C) < 0.01) &&
* (Math.Abs(point.T - stopPoint.T) < 0.01)*/
)
{
break;
}
//計算角度，中心為(500, 0)
double angle = (500 - point.X) / 200.0;
if (angle > 1)
{
angle = 1.0;
}
else if (angle < -1)
{
angle = -1.0;
}
angle = DPoint.Arc2Angle(Math.Asin(angle)) * 2;
//超過一半
if ((point.Y - 0) < 0)
{
angle = 180 + (180 - angle);
}
if (bakAngle == 999)
{
bakAngle = angle;
}
if (maxangle < angle)
{
maxangle = angle;
}
//Console.WriteLine(DateTime.Now.TimeOfDay + ": " + point.X.ToString("000.000") + ", " + point.Y.ToString("000.000") + ", " + point.Z.ToString("000.000") + ", " + point.C.ToString("000.000") + ", " + angle.ToString("000.000") + ", " + (angle - bakAngle).ToString("000.000"));
bakAngle = angle;
if (point_bak == null)
{
point_bak = point;
continue;
}
//幾乎沒有動作，則表示機械手停止
if ((Math.Abs(point.X - point_bak.X) < 0.0001) &&
(Math.Abs(point.Y - point_bak.Y) < 0.0001) &&
(Math.Abs(point.Z - point_bak.Z) < 0.0001) /*&&
* (Math.Abs(point.C - point_bak.C) < 0.0001) &&
* (Math.Abs(point.T - point_bak.T) < 0.0001)*/)
{
MoveStopCnt++;
if (MoveStopCnt > 10)
{
Console.WriteLine("機器手強制停止");
break;
}
}
//else MoveStopCnt = 0;
point_bak = point;
//Thread.Sleep(50);
}
Thread.Sleep(1000);//等待穩定
}