예제 #1
0
        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);//等待穩定
        }