private void Update()
{
_pullInterface.PollDevices();
if (IsShirtActive)
{
var upperModuleAngles = RPY.ParseDataForOrientationAngles(EnfluxPullInterface.ShirtRPY);
if (ShirtState == DeviceState.Initializing && upperModuleAngles.IsInitialized)
{
ShirtState = DeviceState.Streaming;
upperModuleAngles.ApplyUpperAnglesTo(AbsoluteAngles);
ResetFullBodyBaseOrientation();
}
else if (ShirtState == DeviceState.Streaming)
{
upperModuleAngles.ApplyUpperAnglesTo(AbsoluteAngles);
}
}
if (ArePantsActive)
{
var lowerModuleAngles = RPY.ParseDataForOrientationAngles(EnfluxPullInterface.PantsRPY);
if (PantsState == DeviceState.Initializing && lowerModuleAngles.IsInitialized)
{
PantsState = DeviceState.Streaming;
lowerModuleAngles.ApplyLowerAnglesTo(AbsoluteAngles);
ResetFullBodyBaseOrientation();
}
else if (PantsState == DeviceState.Streaming)
{
lowerModuleAngles.ApplyLowerAnglesTo(AbsoluteAngles);
}
}
}
private void ApplyAngles(DeviceType device, ref byte[] rawFrame)
{
var angles = RPY.ParseDataForOrientationAngles(rawFrame);
if (device == DeviceType.Shirt)
{
AbsoluteAngles.SetUpperBodyAngles(
new UnityEngine.Vector3(angles.Center.Roll, angles.Center.Pitch, angles.Center.Yaw) * Mathf.Rad2Deg,
new UnityEngine.Vector3(angles.LeftUpper.Roll, angles.LeftUpper.Pitch, angles.LeftUpper.Yaw) *
Mathf.Rad2Deg,
new UnityEngine.Vector3(angles.LeftLower.Roll, angles.LeftLower.Pitch, angles.LeftLower.Yaw) *
Mathf.Rad2Deg,
new UnityEngine.Vector3(angles.RightUpper.Roll, angles.RightUpper.Pitch, angles.RightUpper.Yaw) *
Mathf.Rad2Deg,
new UnityEngine.Vector3(angles.RightLower.Roll, angles.RightLower.Pitch, angles.RightLower.Yaw) *
Mathf.Rad2Deg);
}
else if (device == DeviceType.Pants)
{
AbsoluteAngles.SetLowerBodyAngles(
new UnityEngine.Vector3(angles.Center.Roll, angles.Center.Pitch, angles.Center.Yaw) * Mathf.Rad2Deg,
new UnityEngine.Vector3(angles.LeftUpper.Roll, angles.LeftUpper.Pitch, angles.LeftUpper.Yaw) *
Mathf.Rad2Deg,
new UnityEngine.Vector3(angles.LeftLower.Roll, angles.LeftLower.Pitch, angles.LeftLower.Yaw) *
Mathf.Rad2Deg,
new UnityEngine.Vector3(angles.RightUpper.Roll, angles.RightUpper.Pitch, angles.RightUpper.Yaw) *
Mathf.Rad2Deg,
new UnityEngine.Vector3(angles.RightLower.Roll, angles.RightLower.Pitch, angles.RightLower.Yaw) *
Mathf.Rad2Deg);
}
}
/**
* @brief RequestTimer start procedure.
*/
private void StartTimerRPY()
{
if (RequestTimerRPY == null)
{
RequestTimerRPY = new Timer(config.SampleTime);
RequestTimerRPY.Elapsed += new ElapsedEventHandler(RequestTimerElapsedRPY);
RequestTimerRPY.Enabled = true;
RPY.ResetAllAxes();
}
}
public void PollDevices()
{
#if UNITY_EDITOR_WIN || UNITY_STANDALONE_WIN
// Check for a new shirt command
if (HasNewCommand(DeviceType.Shirt))
{
var command = PopCommand(DeviceType.Shirt);
var handler = ReceivedShirtStatus;
if (handler != null)
{
handler(command);
}
if (command == InputCommands.DeviceConnected)
{
ShirtStatus = StreamingStatus.Connected;
}
else if (command == InputCommands.DeviceDisconnected)
{
ShirtStatus = StreamingStatus.Disconnected;
}
}
// Check for a new pants command
if (HasNewCommand(DeviceType.Pants))
{
var command = PopCommand(DeviceType.Pants);
var handler = ReceivedPantsStatus;
if (handler != null)
{
handler(command);
}
if (command == InputCommands.DeviceConnected)
{
PantsStatus = StreamingStatus.Connected;
}
else if (command == InputCommands.DeviceDisconnected)
{
PantsStatus = StreamingStatus.Disconnected;
}
}
if (ShirtStatus == StreamingStatus.Connected)
{
//LoadRotations(DeviceType.Shirt, ShirtRotations);
RPY.LoadRotations(DeviceType.Shirt, ShirtRPY);
}
if (PantsStatus == StreamingStatus.Connected)
{
//LoadRotations(DeviceType.Pants, PantsRotations);
RPY.LoadRotations(DeviceType.Pants, PantsRPY);
}
#endif
}
private void UpdatePlotY(double t, double d)
{
LineSeries lineSeries = RPY.Series[2] as LineSeries;
lineSeries.Points.Add(new DataPoint(t, d));
if (lineSeries.Points.Count > config.MaxSampleNumber)
{
lineSeries.Points.RemoveAt(0);
}
if (t >= config.XAxisMax)
{
RPY.Axes[2].Minimum = (t - config.XAxisMax);
RPY.Axes[2].Maximum = t + config.SampleTime / 1000.0;
}
RPY.InvalidatePlot(true);
}