Esempio n. 1
0
    /// <summary>
    /// 复位到点方法
    /// </summary>
    /// <param name="Pos">复位点位置</param>
    /// <param name="Speed">机器移动速度</param>
    /// <param name="Deviation">机器移动误差</param>
    /// <returns></returns>
    public static IEnumerator WaitForResetting(Vector2 Pos, int Speed, int Deviation)
    {
        Debug.Log("开始复位点");
        //发送复位命令
        DynaLinkHS.CmdServoOn();
        DynaLinkHS.CmdLinePassive((int)Pos.x, (int)Pos.y, Speed);
        //等待复位
        float waittime = 0;

        do
        {
            //复位中超过三秒不动重新发点
            if (waittime >= ResendTime)
            {
                DynaLinkHS.CmdServoOn();
                DynaLinkHS.CmdLinePassive((int)Pos.x, (int)Pos.y, Speed);
                waittime = 0;
            }
            yield return(new WaitForSeconds(0.5f));

            //根据MotionInProcess来判断是否在运动
            if (!DiagnosticStatus.MotStatus.MotionInProcess)
            {
                waittime += 0.5f;
            }
            else
            {
                waittime = 0;
            }
        } while ((Mathf.Abs(DynaLinkHS.StatusMotRT.PosDataJ1 - Pos.x) > Deviation || Mathf.Abs(DynaLinkHS.StatusMotRT.PosDataJ2 - Pos.y) > Deviation || DiagnosticStatus.MotStatus.MotionInProcess) && !IsEmrgencyStop && IsConnected && !IsCaution && !IsHalt && !IsPause && !IsFault);
        Debug.Log("Enter");
        yield return("ResettingFinished");
    }