/// <summary>
/// 二次痉挛后清除痉挛，由于立马清除会有问题，延迟0.5秒清除
/// </summary>
void DelayClearAlm()
{
//二次痉挛发生后让电机断电
Debug.Log("二次痉挛清除");
DynaLinkHS.CmdClearAlm();
DynaLinkHS.CmdServoOff();
}
/// <summary>
/// 零点丢失
/// </summary>
public static void CmdHomeCal()
{
DynaLinkHS.CmdClearAlm();
//判断MotFault
//如果有错误是否要clearfault();,之前逻辑这边有通过MotFault去判断
if (DiagnosticStatus.MotStatus.Fault)
{
DynaLinkHS.CmdClearFault();
}
DynaLinkHS.CmdServoOff();
DynaLinkHS.CmdHomeCal();
}
/// <summary>
/// 痉挛复位
/// </summary>
/// <param name="Pos">复位点位置</param>
/// <param name="Speed">复位速度</param>
/// <param name="Deviation">误差</param>
/// <returns></returns>
IEnumerator WaitForSpasmReset(Vector2 Pos, int Speed, int Deviation)
{
//如果有痉挛，清除痉挛
if (CommandFunctions.IsCaution)
{
DynaLinkHS.CmdClearAlm();
}
float waittime = 0;
do
{
yield return(new WaitForSeconds(0.5f));
waittime += 0.5f;
DynaLinkHS.CmdClearAlm();
}while (CommandFunctions.IsCaution && waittime <= 2f);
if (waittime > 2)
{
Debug.Log("清除痉挛失败");
CommandFunctions.spasmState = CommandFunctions.SpasmState.Recovering;
yield break;
}
DynaLinkHS.CmdServoOff();
Ienumerator = CommandFunctions.WaitForResetting(Pos, Speed, Deviation);
//等待复位到上一个点
yield return(StartCoroutine(Ienumerator));
//如果痉挛复位中出现断开连接或者急停，停止复位，并把状态重新切换为在痉挛复位阶段
if (CommandFunctions.IsEmrgencyStop || !CommandFunctions.IsConnected || CommandFunctions.IsCaution)
{
Debug.Log("出现急停或者断开连接,ResettingFailed");
//把状态重新切换为在痉挛复位阶段
CommandFunctions.spasmState = CommandFunctions.SpasmState.Recovering;
yield break;
}
if (CommandFunctions.IsFault)
{
if (CommandFunctions.IsCaution)
{
DynaLinkHS.CmdClearAlm();
}
}
Debug.Log("痉挛复位完成");
//复位完成，痉挛处理过程结束，切换标志位
ControlSpasmAgain = false;
//CommandFunctions.IsSpasmFinished = true;
CommandFunctions.spasmState = CommandFunctions.SpasmState.NoSpasm;
}
void Update()
{
//出现断开连接或者急停或者痉挛，不执行痉挛/复位的任何操作
if (CommandFunctions.IsEmrgencyStop || !CommandFunctions.IsConnected)
{
return;
}
//获得Caution标志位
DetectionManage.Instance.Caution = DiagnosticStatus.MotStatus.Caution;
DetectionManage.Instance.Fault = DiagnosticStatus.MotStatus.Fault;
//痉挛流程处理
switch (CommandFunctions.spasmState)
{
//痉挛等待阶段，等待X秒后开始清除痉挛
case CommandFunctions.SpasmState.RecoveryWaiting:
//等待时间到
if (CommandFunctions.SpasmWaitingTime <= 0)
{
Debug.Log("等待完毕");
//等待时间到，切换痉挛状态为清除痉挛
CommandFunctions.spasmState = CommandFunctions.SpasmState.Recovering;
}
else
{
CommandFunctions.SpasmWaitingTime -= Time.deltaTime;
}
break;
//清除痉挛阶段
case CommandFunctions.SpasmState.Recovering:
//首先判断是在复位中还是游戏中
//如果在复位中继续复位
if (CommandFunctions.IsInReset)
{
Debug.Log("IsInReset");
ControlSpasmAgain = false;
//清除痉挛
DynaLinkHS.CmdClearAlm();
DynaLinkHS.CmdServoOff();
DynaLinkHS.CmdServoOn();
//改变痉挛完成标志位
//CommandFunctions.IsSpasmFinished = true;
CommandFunctions.spasmState = CommandFunctions.SpasmState.NoSpasm;
}
else
{
//在游戏中，改变痉挛状态为等待复位中
Debug.Log("IsInGame");
CommandFunctions.spasmState = CommandFunctions.SpasmState.ResettingWaiting;
//开始复位到上一个点
StartCoroutine(WaitForSpasmReset(CommandFunctions.SpasmPostion, CommandFunctions.ResetSpeed, CommandFunctions.ResetDeviation));
}
break;
//等待不做任何事
case CommandFunctions.SpasmState.ResettingWaiting:
break;
//二次痉挛
case CommandFunctions.SpasmState.SpasmAgain:
Debug.Log("二次痉挛");
//停止所有协程
StopAllCoroutines();
//等待清除痉挛标志位
WaitforCaution = true;
//延迟0.5秒清除痉挛
Invoke("DelayClearAlm", 0.5f);
CommandFunctions.spasmState = CommandFunctions.SpasmState.ResettingWaiting;
break;
//跳过痉挛
case CommandFunctions.SpasmState.SkipSpasm:
Debug.Log("跳过痉挛");
CommandFunctions.spasmState = CommandFunctions.SpasmState.ResettingWaiting;
//停止现在所有协程
StopAllCoroutines();
if (Ienumerator != null)
{
StopCoroutine(Ienumerator);
}
DynaLinkHS.CmdServoOn();
CommandFunctions.SpasmWaitingTraining = 0;
//如果还有痉挛清除痉挛
if (CommandFunctions.IsCaution)
{
WaitforCaution = true;
DynaLinkHS.CmdClearAlm();
DynaLinkHS.CmdServoOn();
}
else
{
//痉挛已清除，直接改变痉挛过程完成标志位
ControlSpasmAgain = false;
//CommandFunctions.IsSpasmFinished = true;
//设置痉挛状态为无痉挛
CommandFunctions.spasmState = CommandFunctions.SpasmState.NoSpasm;
}
break;
}
//复位流程处理
if (CommandFunctions.resettingState != CommandFunctions.ResettingState.ResetFinshed && CommandFunctions.IsCanReset && !CommandFunctions.IsCaution && !CommandFunctions.IsPause && DiagnosticStatus.MotStatus.SysCalPass)
{
switch (CommandFunctions.resettingState)
{
//游戏开始前复位
case CommandFunctions.ResettingState.Resetting:
CommandFunctions.resettingState = CommandFunctions.ResettingState.ResetWaiting;
//开始复位到游戏开始点
StartCoroutine(WaitForReset(CommandFunctions.ResetPostion, CommandFunctions.ResetSpeed, CommandFunctions.ResetDeviation));
break;
//等待阶段什么都不做
case CommandFunctions.ResettingState.ResetWaiting:
break;
//游戏结束后复位
case CommandFunctions.ResettingState.EndResetting:
CommandFunctions.resettingState = CommandFunctions.ResettingState.ResetWaiting;
//开始复位到游戏结束点
StartCoroutine(WaitForEndReset(CommandFunctions.ResetPostion, CommandFunctions.ResetSpeed, CommandFunctions.ResetDeviation));
break;
}
}
}
