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!
}
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;
}
