/// <summary> /// Handles the client and get movement. /// </summary> /// <param name="client">Client.</param> void HandleClientComm(object client) { Message = string.Empty; TcpClient tcpClient = (TcpClient)client; NetworkStream clientStream = tcpClient.GetStream(); byte[] message = new byte[bufferSize]; int bytesRead; while (true) { bytesRead = 0; try { // recive message bytesRead = clientStream.Read(message, 0, bufferSize); } catch { // a socket error has occured break; } if (bytesRead == 0) { // the client has disconnected from the server break; } Message = DecodeMsg(message); //this variable is unnecessary. should be deleted! UnityEngine.Debug.LogWarning("Msg: " + MsgByte.Operation + MsgByte.val1.ToString() + MsgByte.val2.ToString() + MsgByte.val3.ToString()); //fires the message event if (incomingMessage != null) { incomingMessage(MsgByte); } movement = GetMovement(MsgByte); //WaitForAck = false; while (WaitForAck) { System.Threading.Thread.Sleep(10); } WaitForAck = true; // send acknowledge byte clientStream.WriteByte(AckByte); } }
private void HandleIncomingMessage(TCPServer.ByteCast msg, bool manualControl = false) { DOF CloseControl = DOF_ArmList.Where(x => x.ID == 0x09).ElementAt(0); DOF PincerControl = DOF_ArmList.Where(x => x.ID == 0x0C).ElementAt(0); DOF IndexControl = DOF_ArmList.Where(x => x.ID == 0x0A).ElementAt(0); DOF KeyControl = DOF_ArmList.Where(x => x.ID == 0x0D).ElementAt(0); if (!UseManualControl || manualControl || msg.Operation != 0x01) { switch (msg.Operation) { //Control main arm case 0x01: //update heatmap only if heat is true to prevent errors if (MuscularActivityMap && msg.heatmap != null) { UpdateMuscularActivityMap = true; CurrentMuscularActivityMap = msg.heatmap; } DOF dofArm = DOF_ArmList.Where(x => x.ID == msg.val1).ElementAt(0); if (!BlockedMovment.Contains(serverInstance.movement)) { if (CloseMovement.Contains(msg.val1) && CloseControl.CurrentValue > 0) { CloseControl.Move(msg.val3, 0x0); } else if (msg.val1 == 0x09 && msg.val2 == 0x01 && (PincerControl.CurrentValue > 0 || CloseControl.CurrentValue < 0 || IndexControl.CurrentValue > 0 || KeyControl.CurrentValue > 0)) { if (PincerControl.CurrentValue > 0) { PincerControl.Move(msg.val3, 0x0); } if (CloseControl.CurrentValue < 0) { CloseControl.Move(msg.val3, 0x01); } if (IndexControl.CurrentValue > 0) { IndexControl.Move(msg.val3, 0x0); } if (KeyControl.CurrentValue > 0) { KeyControl.Move(msg.val3, 0x0); } } else { dofArm.Move(msg.val3, msg.val2); } } //fire event OnMotion. Check if someone registered to this event. //this can be simplified by OnMotion?.Invoke(...); If performance drops try the simplification! if (OnMotion != null) { OnMotion(new HandMotion(serverInstance.movement, dofArm.CurrentValue, msg.val3 / 100f)); } break; //Control TAC arm case 0x02: throw new Exception("TAC-Test not supported"); // Control the training phase case 0x03: //TODO break; //Reset the main arm case 0x72: switch (msg.val1) { case 0x74: throw new Exception("TAC-Test not supported"); default: foreach (var dof in DOF_ArmList) { dof.RestValue(); } break; } break; // Configurations case 0x63: usePalmarGrasp = false; switch (msg.val1) { case 0x01: // Select upper limb or lower limb //TODO new NotImplementedException("Selecting upper limb or lower limb"); break; case 0x02: throw new Exception("TAC-Test not supported"); case 0x03: throw new Exception("TAC-Test not supported"); case 0x05: // Switch between right and left limb RotateArm = true; break; default: //TODO break; } break; default: //TODO break; } } serverInstance.SendAck(0x01); //set current movement and current movement's velocity currentMovement = serverInstance.movement; currentForce = msg.val3 / 100f; //maybe this is a source for errors. Needs to be tested! }
public HandMotion(HandMovement.Movement motion, float limit, float force) { Motion = motion; Force = force; Limit = limit; }
private void HandleIncomingMessage(TCPServer.ByteCast msg, bool manualControl = false) { DOF CloseControl = DOF_ArmList.Where(x => x.ID == 0x09).ElementAt(0); DOF PincerControl = DOF_ArmList.Where(x => x.ID == 0x0C).ElementAt(0); DOF IndexControl = DOF_ArmList.Where(x => x.ID == 0x0A).ElementAt(0); DOF KeyControl = DOF_ArmList.Where(x => x.ID == 0x0D).ElementAt(0); print(msg.val1.ToString()); if (!UseManualControl || manualControl || msg.Operation != 0x01) { switch (msg.Operation) { //Control main arm case 0x01: if (StartTAC) { GlowArm = true; } //update heatmap only if heat is true to prevent errors if (MuscularActivityMap && msg.heatmap != null) { UpdateMuscularActivityMap = true; CurrentMuscularActivityMap = msg.heatmap; } DOF dofArm = DOF_ArmList.Where(x => x.ID == msg.val1).ElementAt(0); if (!BlockedMovment.Contains(serverInstance.movement)) { if (CloseMovement.Contains(msg.val1) && CloseControl.CurrentValue > 0) { CloseControl.Move(msg.val3, 0x0); } else if (msg.val1 == 0x09 && msg.val2 == 0x01 && (PincerControl.CurrentValue > 0 || CloseControl.CurrentValue < 0 || IndexControl.CurrentValue > 0 || KeyControl.CurrentValue > 0)) { if (PincerControl.CurrentValue > 0) { PincerControl.Move(msg.val3, 0x0); } if (CloseControl.CurrentValue < 0) { CloseControl.Move(msg.val3, 0x01); } if (IndexControl.CurrentValue > 0) { IndexControl.Move(msg.val3, 0x0); } if (KeyControl.CurrentValue > 0) { KeyControl.Move(msg.val3, 0x0); } } else { dofArm.Move(msg.val3, msg.val2); } } //fire event OnMotion. Check if someone registered to this event. //this can be simplified by OnMotion?.Invoke(...); If performance drops try the simplification! if (OnMotion != null) { OnMotion(new HandMotion(serverInstance.movement, dofArm.CurrentValue, msg.val3 / 100f)); } break; //Control TAC arm case 0x02: HideTACArm = false; MainArmLayer = "Default"; TACArmSkinR = !HideTACArm; DOF dofTAC = DOF_TAC_ArmList.Where(x => x.ID == msg.val1).ElementAt(0); dofTAC.Move(msg.val3 * 4, msg.val2); break; // Control the training phase case 0x03: // TODO break; //Reset the main arm case 0x72: switch (msg.val1) { case 0x74: foreach (var dof in DOF_TAC_ArmList) { dof.RestValue(); } MainArmLayer = "Default"; break; default: foreach (var dof in DOF_ArmList) { dof.RestValue(); } MainArmLayer = "Default"; break; } break; // Configuration case 0x63: usePalmarGrasp = false; switch (msg.val1) { case 0x01: // Select upper limb or lower limb //TODO new NotImplementedException("Selecting upper limb or lower limb"); break; case 0x02: // Switch TAC test on/off StartTAC = !StartTAC; TACArmSkinR = StartTAC; MainArmLayer = "Default"; break; case 0x03: //Error tolerance for the TAC test Allowance = (float)(Convert.ToInt32(msg.val2)) / 50; break; case 0x05: // Switch between right and left limb RotateArm = true; break; default: //TODO break; } break; // Send text/Timer case 0x74: string text = System.Text.Encoding.ASCII.GetString(msg.heatmap).Trim(new char[] { ' ', (char)0 }); var motion = TCPServer.GetMovement(msg).ToString(); var min = Convert.ToInt32(msg.val3); var sec = Convert.ToInt32(msg.val4); var tt = string.Format("{0} {1} in {2}:{3}", text, motion, min, sec); print(tt); break; default: //TODO break; } } serverInstance.Message = string.Empty; bool overPostion = false; for (int i = 0; i < DOF_ArmList.Count; i++) { if (DOF_ArmList[i] != null) { if (Mathf.Abs(DOF_ArmList[i].CurrentValue - DOF_TAC_ArmList[i].CurrentValue) > Allowance) { overPostion = true; break; } } else { break; } } if (!overPostion && StartTAC && DOF_ArmList.Any(x => Math.Abs(x.CurrentValue) > 0.1)) { HideTACArm = true; if (GlowArm) { MainArmLayer = "Outline"; } serverInstance.SendAck(0x74); } else { if (StartTAC) { MainArmLayer = "Default"; HideTACArm = false; } serverInstance.SendAck(0x01); } TACArmSkinR = !HideTACArm; //set current movement and current movement's velocity currentMovement = serverInstance.movement; currentForce = msg.val3 / 100f; }