/// <summary>
/// 二次痉挛后清除痉挛，由于立马清除会有问题，延迟0.5秒清除
/// </summary>
void DelayClearAlm()
{
//二次痉挛发生后让电机断电
Debug.Log("二次痉挛清除");
DynaLinkHS.CmdClearAlm();
DynaLinkHS.CmdServoOff();
}
/// <summary>
/// Send command when it is assist.
/// </summary>
public void SendCmdWhenAssist()
{
if (_currentGameRecorder.TrainType == 2)
{
DynaLinkHS.CmdServoOff();
}
}
void GameStart(object source, System.Timers.ElapsedEventArgs e)
{
DynaLinkHS.CmdServoOff();
RunFSM.currState = RunFSM.GameState_Enum.STATE_BREAK; // initial state
StateFunc.breakTimer = SetYaml.breakTimeSet; // break timer
print("初始化结束");
}
private void OnGUI()
{
if (!CanShowGUI)
{
return;
}
GUILayout.Space(40);
if (!DynaLinkHS.ServerLinkActBit && !MainCore.Instance.IsMachineDisabled)
{
if (GUILayout.Button(" 重连 ", GUILayout.Width(160), GUILayout.Height(60)))
{
StartCoroutine(OnReconnect());
}
}
if (GUILayout.Button(" 断开 ", GUILayout.Width(160), GUILayout.Height(60)))
{
if (!MainCore.Instance.IsMachineDisabled)
{
DynaLinkHS.CmdServoOff();
}
DynaLinkCore.StopSocket();
Application.Quit();
}
}
private static void startMov1(object source, System.Timers.ElapsedEventArgs e)
{
DynaLinkHS.CmdTransparentControl(0.2f, 0.2f, 1.0f, 1.0f, 10.0f, 10.0f, 10.0f, 10.0f);
DynaLinkHS.CmdServoOff();
SetYaml.LoadTaskInfo();// load .yaml file
print("初始化完成，实验开始");
// first block set
RunFSM.currState = RunFSM.GameState_Enum.STATE_BLOCKSET;
}
public static void Break()
{
string blockName = SetYaml.taskNames[blockNum];
//save sEMG
string fileNamesemg = SetYaml.subjectName + "_" + blockName + "_" + GlobalVar.sEMGCSVNAME + "_" + Convert.ToString(trials) + ".csv";
string csvfullemg = System.Environment.CurrentDirectory + "\\" + GlobalVar.expFolderName + "\\" + fileNamesemg;
if (RunFSM.EMG.IsConnected())
{
if (isEMGRecording == false)
{
RunFSM.EMG.StopRecording(csvfullemg);
isEMGRecording = true;
}
}
if (trials >= SetYaml.trialNum)
{
timeLine += Time.deltaTime;
csvContent.AppendLine((float)timeLine + "," + trials + "," + "TrialBreak" + "," + DynaLinkHS.StatusRobot.PositionDataJoint1 + "," + DynaLinkHS.StatusRobot.PositionDataJoint2);
//save kinematics
string fileName = SetYaml.subjectName + "_" + blockName + "_" + GlobalVar.CSVNAME + ".csv";
string csvfullfilename = System.Environment.CurrentDirectory + "\\" + GlobalVar.expFolderName + "\\" + fileName;
File.WriteAllText(csvfullfilename, "Time,Trial,State,PositionX,PositionY\n");
File.AppendAllText(csvfullfilename, csvContent.ToString());
// finish 4 blocks
if (blockNum >= 3)
{
Application.Quit();
}
else
{
RunFSM.currState = RunFSM.GameState_Enum.STATE_BLOCKSET;
blockTimer = 5f;
}
}
else
{
timeLine += Time.deltaTime;
csvContent.AppendLine((float)timeLine + "," + trials + "," + "TrialBreak" + "," + DynaLinkHS.StatusRobot.PositionDataJoint1 + "," + DynaLinkHS.StatusRobot.PositionDataJoint2);
breakTimer -= Time.deltaTime; // break timer --
if (breakTimer <= 0f)
{
DynaLinkHS.CmdServoOff();
DynaLinkHS.CmdTransparentControl(0.2f, 0.2f, 1.0f, 1.0f, 10.0f, 10.0f, 10.0f, 10.0f);
// next trial set
isCollisionOn = false;
isCollisionOff = false;
m2MoveTimer = 5f;
RunFSM.currState = RunFSM.GameState_Enum.STATE_CMD2ROBOT;
}
}
}
/// <summary>
/// 零点丢失
/// </summary>
public static void CmdHomeCal()
{
DynaLinkHS.CmdClearAlm();
//判断MotFault
//如果有错误是否要clearfault();,之前逻辑这边有通过MotFault去判断
if (DiagnosticStatus.MotStatus.Fault)
{
DynaLinkHS.CmdClearFault();
}
DynaLinkHS.CmdServoOff();
DynaLinkHS.CmdHomeCal();
}
void CloseUIs()
{
// Stop machine.
DynaLinkHS.CmdServoOff();
// Close all UI
CloseUIForm(HiddenObjectPage.MenuPage, true);
CloseUIForm(HiddenObjectPage.PromptTyingHandPage, true);
CloseUIForm(HiddenObjectPage.ResettingPage, true);
CloseUIForm(HiddenObjectPage.CountDownPage, true);
CloseUIForm(HiddenObjectPage.StartTrainPage, true);
CloseUIForm(HiddenObjectPage.HOGUICharacter, true);
CloseUIForm(HiddenObjectPage.HOGUITop, true);
CloseUIForm(HiddenObjectPage.HOGUIGameOver, true);
}
/// <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;
}
public static void Move2Start()
{
m2MoveTimer -= Time.deltaTime;
if (m2MoveTimer < 0)
{
timeLine += Time.deltaTime;
csvContent.AppendLine((float)timeLine + "," + trials + "," + "M2Reset" + "," + DynaLinkHS.StatusRobot.PositionDataJoint1 + "," + DynaLinkHS.StatusRobot.PositionDataJoint2);
DynaLinkHS.CmdServoOff();
ShowCircle.EndPointCircle(blockNum);
RunFSM.currState = RunFSM.GameState_Enum.STATE_TRIALSTART;
}
else
{
timeLine += Time.deltaTime;
csvContent.AppendLine((float)timeLine + "," + trials + "," + "M2Reset" + "," + DynaLinkHS.StatusRobot.PositionDataJoint1 + "," + DynaLinkHS.StatusRobot.PositionDataJoint2);
}
}
public override void Begin(int flow)
{
switch (flow)
{
case (int)HiddenObjectFlow.Start:
Debug.Log("Flow: Start");
Next();
break;
case (int)HiddenObjectFlow.CountDown:
Debug.Log("Flow: CountDownWarmUp");
// Countdown
StartCoroutine(CountDown());
break;
case (int)HiddenObjectFlow.Training:
Debug.Log("Flow: Training");
// Start game
GameStart.Instance.CanStartGame = true;
EventManager.Instance.PublishEvent(EventTypeSet.GameStart, new EventData(this));
// Close page of message
//GameStart.Instance.OpenOrCloseMessagePage(false, HiddenObjectPage.StartTrainPage);
// Send machine cmd
GameStart.Instance.SendMachineCmd();
break;
case (int)HiddenObjectFlow.End:
Debug.Log("Flow: End");
DynaLinkHS.CmdServoOff();
EventManager.Instance.PublishEvent(EventTypeSet.FinishGame, new EventData(this));
StartCoroutine(EndFlowResetting());
if (_musicController)
{
_musicController.StopBGM();
}
//Next();
break;
default:
Debug.LogWarning("Unexist flow! " + flow);
break;
}
}
public override void OnStateChanged(PlayerState nextState)
{
DynaLinkHS.CmdServoOff();
}
private void CarGameStart(object source, System.Timers.ElapsedEventArgs e)
{
DynaLinkHS.CmdServoOff();
isCarGame = true;
}
void StopBtnClick()
{
DynaLinkHS.CmdServoOff();
}
void HomeCalBtnClick()
{
DynaLinkHS.CmdClearAlarm();
DynaLinkHS.CmdServoOff();
DynaLinkHS.CmdFindHomePosition();
}
private static void startMov(object source, System.Timers.ElapsedEventArgs e)
{
DynaLinkHS.CmdServoOff();
}
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;
}
}
}