/// <summary>
/// Send command when it is assist.
/// </summary>
public void SendCmdWhenAssist()
{
if (_currentGameRecorder.TrainType == 2)
{
DynaLinkHS.CmdServoOff();
}
}
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();
}
}
/// <summary>
/// 二次痉挛后清除痉挛，由于立马清除会有问题，延迟0.5秒清除
/// </summary>
void DelayClearAlm()
{
//二次痉挛发生后让电机断电
Debug.Log("二次痉挛清除");
DynaLinkHS.CmdClearAlm();
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("初始化结束");
}
/// <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");
}
void OnTriggerExit2D(Collider2D collider)
{
//Debug.Log("Triggle Exit Collider：" + collider.gameObject.name);
if (collider.gameObject.name == "CollisionYP-CCW")
{
//Debug.Log("Exit：CollisionYP_CCW");
DynaLinkHS.CmdSetCurrentPositionAsEndEffectorLimitPosition(0x00);
DynaLinkHS.CmdSetCurrentPositionAsEndEffectorLimitPosition(0x00);
}
else if (collider.gameObject.name == "CollisionYN-CW")
{
//Debug.Log("Exit：CollisionYN_CW");
DynaLinkHS.CmdSetCurrentPositionAsEndEffectorLimitPosition(0x00);
DynaLinkHS.CmdSetCurrentPositionAsEndEffectorLimitPosition(0x00);
}
else if (collider.gameObject.name == "CollisionXP-CCW")
{
//Debug.Log("Exit：CollisionXP_CCW");
DynaLinkHS.CmdSetCurrentPositionAsEndEffectorLimitPosition(0x00);
DynaLinkHS.CmdSetCurrentPositionAsEndEffectorLimitPosition(0x00);
}
else if (collider.gameObject.name == "CollisionXN-CW")
{
//Debug.Log("Exit：CollisionXN_CW");
DynaLinkHS.CmdSetCurrentPositionAsEndEffectorLimitPosition(0x00);
DynaLinkHS.CmdSetCurrentPositionAsEndEffectorLimitPosition(0x00);
}
}
//Stop Socket connect opereation
public static void StopSocket()
{
DynaLinkHS.Disconnect();
// DynaLinkHS.CmdInitOp(0x02);//Send close net work request
print("Prepare to Close TCP");
// UdpCloseBit = true;
}
/////////////////////////////////////////////////////////////////////////////
//Socket application
//Socket operation code area start
//Do not Change IP address and Port number!
/////////////////////////////////////////////////////////////////////////////
public static void ConnectClick()
{
UdpSocketClient.InitSocket();
Local_delay();
DynaLinkHS.SocketDataLogic();
DynaLinkHS.CmdInitOp(0x01);//Init Net work request
print("UDP port OPEN");
}
IEnumerator WaitTime()
{
yield return(new WaitForSeconds(0.8f));
//Must wait 0.8 sec befor close the UDP connection.
//DynaLinkHS.CoreThreadLoopBit = false;
DynaLinkHS.Disconnect();
print("TCP port Closed Sucess");
}
// Update is called once per frame
void FixedUpdate()
{
DynaLinkHS.EthernetStatusWDG();
if (UdpCloseBit == true)
{
StartCoroutine(WaitTime());
UdpCloseBit = false;
}
}
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;
}
/////////////////////////////////////////////////////////////////////////////
//Socket application
//Socket operation code area start
//Do not Change IP address and Port number!
/////////////////////////////////////////////////////////////////////////////
public static void ConnectClick()
{
DynaLinkHS.Connect();
// Local_delay();
// DynaLinkHS.SocketDataLogic();
// DynaLinkHS.CmdInitOp(0x01);//Init Net work request
print("TCP port OPEN");
}
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;
}
}
}
public static void TaskSucceed()
{
ShowCircle.aimObject.GetComponent <Renderer>().enabled = false; // circle vanish
// record data
timeLine += Time.deltaTime;
csvContent.AppendLine((float)timeLine + "," + trials + "," + "TrialSucceed" + "," + DynaLinkHS.StatusRobot.PositionDataJoint1 + "," + DynaLinkHS.StatusRobot.PositionDataJoint2);
trials += 1;
breakTimer = SetYaml.breakTimeSet;// breaktimer start
// 【trial succeed】transfer to【break】
DynaLinkHS.CmdPauseMotion();
RunFSM.currState = RunFSM.GameState_Enum.STATE_BREAK;
}
/// <summary>
/// 零点丢失
/// </summary>
public static void CmdHomeCal()
{
DynaLinkHS.CmdClearAlm();
//判断MotFault
//如果有错误是否要clearfault();,之前逻辑这边有通过MotFault去判断
if (DiagnosticStatus.MotStatus.Fault)
{
DynaLinkHS.CmdClearFault();
}
DynaLinkHS.CmdServoOff();
DynaLinkHS.CmdHomeCal();
}
private void OnCarGameBtnClick()
{
isCarGame = false;
timerInit = new System.Timers.Timer();
timerInit.Interval = 10000; //Wait for 10 seconds
timerInit.Elapsed += CarGameStart; //Hook up the elapsed event for the timer
timerInit.AutoReset = false; //Have the timer fire repeated events(true is the default)
timerInit.Enabled = false;
timerInit.Start();
float M2_SIZE_X = (float)(0.551 + 0.022) / 2;
DynaLinkHS.CmdPassiveMovementControl((-0.022f + 0.551f) / 2, (0.02f + 0.2417f) / 2, (float)0.05);
}
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);
}
// Use this for initialization
void Start()
{
ScreenSize.ScreenSet();
// center point
//DynaLinkHS.CmdPassiveMovementControl(ScreenSize.SCREEN_MID_X, ScreenSize.SCREEN_MID_X, (float)0.05);
//DynaLinkHS.CmdPassiveMovementControl(ScreenSize.SCREEN_MID_X, ScreenSize.SCREEN_MID_Y, (float)0.05);
DynaLinkHS.CmdFindHomePosition();
timerRealse = new Timer();
timerRealse.Interval = 10000; //Wait for 10 seconds
timerRealse.Elapsed += startMov; //Hook up the elapsed event for the timer
timerRealse.AutoReset = false; //Have the timer fire repeated events(true is the default)
timerRealse.Enabled = false;
timerRealse.Start();
}
/// <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 Cmd2Robot()
{
// CMD: M2 move to starting point
string taskPointKey = SetYaml.taskNames[blockNum];
float[] taskPointValue = new float[2];
SetYaml.startPointDic.TryGetValue(taskPointKey, out taskPointValue);
print(taskPointKey);
DynaLinkHS.CmdPassiveMovementControl(taskPointValue[0], taskPointValue[1], (float)0.08);
timeLine += Time.deltaTime;
csvContent.AppendLine((float)timeLine + "," + trials + "," + "CMDM2Reset" + "," + DynaLinkHS.StatusRobot.PositionDataJoint1 + "," + DynaLinkHS.StatusRobot.PositionDataJoint2);
RunFSM.currState = RunFSM.GameState_Enum.STATE_MOVE2START;
RunFSM.EMG.StartRecording();// start record sEMG
}
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);
}
}
// Use this for initialization
void Start()
{
DynaLinkHS.CmdTransparentControl(5f, 5f, 10f, 10f, 10f, 10f, 10f, 10f);
//DynaLinkHS.CmdServoOff();
///*
// Initiate Hand Effector Position
///*
screenSize = Camera.main.ScreenToWorldPoint(new Vector2(Screen.width, Screen.height)); // 尺寸的一半
print(screenSize); // screen size
float M2_SIZE_X = (M2_AXIS_X[1] - M2_AXIS_X[0]) / 2;
float M2_SIZE_Y = (M2_AXIS_Y[1] - M2_AXIS_Y[0]) / 2;
xOffset = (screenSize.x) / M2_SIZE_X;
yOffset = (screenSize.y) / M2_SIZE_Y;
DynaLinkHS.CmdPassiveMovementControl((M2_AXIS_X[1] + M2_AXIS_X[0]) / 2, (M2_AXIS_Y[1] + M2_AXIS_Y[0]) / 2, (float)0.05);
///*
// Initiate Control Mode(mass/damping/spring)
/// for M2, the parameters are
/// - origin/target position x [原始/目标位置 x 轴] (type : float, unit : m, range : )
/// - origin/target position y [原始/目标位置 y 轴] (type : float, unit : m, range : )
/// - M (mass) x [模拟质量 x 轴] (type : float, unit : kg, range : )
/// - M (mass) y [模拟质量 y 轴] (type : float, unit : kg, range : )
/// - B (damping) x [模拟阻尼 x 轴] (type : float, unit : N/(m/s), range : )
/// - B (damping) y [模拟阻尼 y 轴] (type : float, unit : N/(m/s), range : )
/// - K (spring) x [模拟弹簧 x 轴] (type : float, unit : N/m, range : )
/// - K (spring) y [模拟弹簧 y 轴] (type : float, unit : N/m, range : )
///*
///*
/// Renew/Load .yaml
///*
//SetYaml.IniTaskInfo();// initiate .yaml
SetYaml.LoadTaskInfo();// load .yaml
timerStart = new Timer();
timerStart.Interval = 10000; //Wait for 10 seconds
timerStart.Elapsed += GameStart; //Hook up the elapsed event for the timer
timerStart.AutoReset = false; //Have the timer fire repeated events(true is the default)
timerStart.Enabled = false;
timerStart.Start();
}
private void Awake()
{
// 初始化窗体属性
base.CurrentUIType.UIForms_Type = UIFormType.PopUp;
base.CurrentUIType.UIForms_ShowMode = UIFormShow.ReverseChange;
base.CurrentUIType.UIForm_LucencyType = UIFormLucenyType.ImPenetrable;
//base.Initialize();
// 注册按钮事件
RigisterButtonObjectEvent("CountDown_Pause",
p =>
{
OpenUIForm("HOGUIPause");
GlobalApplication.IsPause = true;
DynaLinkHS.CmdServoOn();
SendMessage(HiddenObjectMessage.MsgStopGame, "", "True");
}
);
}
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;
}
}
void Awake()
{
base.CurrentUIType.UIForms_Type = UIFormType.Normal;
base.CurrentUIType.UIForms_ShowMode = UIFormShow.Normal;
base.CurrentUIType.UIForm_LucencyType = UIFormLucenyType.Lucency;
//注册按钮事件
RigisterButtonObjectEvent("Btn_Music",
p =>
{
// print("音乐按钮");
GlobalApplication.IsPause = true;
OpenUIForm(HiddenObjectPage.MusicConsole);
DynaLinkHS.CmdServoOn();
SendMessage(HiddenObjectMessage.MsgStopGame, "", "True");
}
);
RigisterButtonObjectEvent("Btn_Pause",
p =>
{
GlobalApplication.IsPause = true;
OpenUIForm("HOGUIPause");
DynaLinkHS.CmdServoOn();
SendMessage(HiddenObjectMessage.MsgStopGame, "", "True");
}
);
RigisterButtonObjectEvent("Btn_Next",
p =>
{
print("下一步按钮");
//CloseUIForm("Task");
OnClickNext();
}
);
}
void UpdateDOBtnClick()
{
DynaLinkHS.CmdSetDigitOutput(StatusODL[0], StatusODL[1], StatusODL[2], StatusODL[3]);
}
void ClearAlmClick()
{
DynaLinkHS.CmdClearAlarm();
}
void onClickUpgradeAppButton()
{
filePath = FilePathInputField.text;
DynaLinkHS.UpgradeMMUApp(filePath);
}
private void CarGameStart(object source, System.Timers.ElapsedEventArgs e)
{
DynaLinkHS.CmdServoOff();
isCarGame = true;
}
void onClickRunAppButton()
{
DynaLinkHS.CmdSetBootMode(DynaLinkHSPara.IAPBootMode.APP);
}
