/// <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");
}