/// <summary>
/// Generates an updated reference for the given device channel.
/// Reads tracking data from a set of sensors.
/// Generates references with the active reference generator.
/// Should only be called during Physics updates, Monobehaviour : FixedUpdate.
/// </summary>
/// <param name="channel">The channel number.</param>
/// <returns>The updated reference.</returns>
public override float GenerateReference(int channel)
{
// Jacobian synergy reference generator requires multiple sensors.
if (GetActiveReferenceGeneratorType() == ReferenceGeneratorType.JacobianSynergy)
{
// Save currently active sensor
SensorType prevSensorType = activeSensor.GetSensorType();
// Get shoulder position and velocity
Configure("CMD_SET_ACTIVE_SENSOR", SensorType.VIVETracker);
float qShoulder = activeSensor.GetProcessedData(5) + Mathf.PI / 2; // Offsetting to horizontal position being 0.
float qDotShoulder = activeSensor.GetProcessedData(0);
// Get elbow position
Configure("CMD_SET_ACTIVE_SENSOR", SensorType.VirtualEncoder);
float qElbow = activeSensor.GetProcessedData(0);
// Get enable
Configure("CMD_SET_ACTIVE_SENSOR", SensorType.VIVEController);
float enableValue = activeSensor.GetProcessedData(1);
// Combine input
float[] input = { qShoulder, -qElbow, qDotShoulder, enableValue };
//Debug.Log("The input is: qs = " + Mathf.Rad2Deg * input[0] + ", qe = " + Mathf.Rad2Deg * input[1] + ", qDotS = " + input[2] + ", enable = " + input[3]);
// Go back to previously active sensor
Configure("CMD_SET_ACTIVE_SENSOR", prevSensorType);
// Update enable
isEnabled = activeGenerator.IsEnabled();
// Generate reference
return(activeGenerator.UpdateReference(channel, input));
}
// Linear synergy reference generator requires multiple sensors.
else if (GetActiveReferenceGeneratorType() == ReferenceGeneratorType.LinearKinematicSynergy)
{
// Save currently active sensor
SensorType prevSensorType = activeSensor.GetSensorType();
// Get residual limb velocity
Configure("CMD_SET_ACTIVE_SENSOR", SensorType.VIVETracker);
float qDotShoulder = activeSensor.GetProcessedData(0);
// Get enable
Configure("CMD_SET_ACTIVE_SENSOR", SensorType.VIVEController);
float enableValue = activeSensor.GetProcessedData(1);
// Combine input
float[] input = { qDotShoulder, enableValue };
//Debug.Log("The input is: qs = " + Mathf.Rad2Deg * input[0] + ", qe = " + Mathf.Rad2Deg * input[1] + ", qDotS = " + input[2] + ", enable = " + input[3]);
// Go back to previously active sensor
Configure("CMD_SET_ACTIVE_SENSOR", prevSensorType);
// Update enable
isEnabled = activeGenerator.IsEnabled();
// Generate reference
return(activeGenerator.UpdateReference(channel, input));
}
else if (GetActiveReferenceGeneratorType() == ReferenceGeneratorType.EMGInterface)
{
// Save currently active sensor
SensorType prevSensorType = activeSensor.GetSensorType();
// Get EMG status
Configure("CMD_SET_ACTIVE_SENSOR", emgSensorType);
float[] emgStatus = activeSensor.GetAllProcessedData();
// Get enable
Configure("CMD_SET_ACTIVE_SENSOR", SensorType.VIVEController);
float enableValue = activeSensor.GetProcessedData(1);
// Combine input
List <float> input = new List <float>();
input.Add(enableValue);
foreach (float emgState in emgStatus)
{
input.Add(emgState);
//Debug.Log(emgState);
}
// Go back to previously active sensor
Configure("CMD_SET_ACTIVE_SENSOR", prevSensorType);
// Update enable
isEnabled = activeGenerator.IsEnabled();
// Generate reference
return(activeGenerator.UpdateReference(channel, input.ToArray()));
}
else
{
// First read the angular velocity from sensor
float[] input = { activeSensor.GetProcessedData(channel) };
// Compute reference with that angular velocity
return(activeGenerator.UpdateReference(channel, input));
}
}
