void FixedUpdate()
{
initialise += 1;
// Enable the gyro and compass data
Input.gyro.enabled = true;
Input.compass.enabled = true;
// Wait till all the sensors have warmed up before using any readings
if ((initialise > 2 / 0.02) && (startStopPedometer.text == "Stop"))
{
//Graphing update values
// tempList = pedometer.value_history.OfType<float>().ToList();
// window.updateValues(tempList);
// filtered_graph.updateValues(tempList);
// Log the time and acceleration data
string dl_data = System.DateTime.Now.ToString();
string disp_data = "";
this.updateAllClasses();
// Display the data to GUI
// Pedometer data
disp_data = "Currently at " + pedometer.numSteps + " steps \n" +
// "V1: Dist = " + 0.8* pedometer.numSteps + " meters \n" +
// "V2: Dist = " + pedometer.distance_travelled + " meters \n" +
"Distance travelled:" + pedometer.distance_travelled + " meters \n";
// "max_pulse_magnitude = " + pedometer.max_pulse_magnitude + " \n\n";
// Orientation data raw from sensors
// disp_data += string.Format("Gyro data: x: {0:F4}, y: {1:F4}, z: {2:F4}.\n", Input.gyro.rotationRate.x, Input.gyro.rotationRate.y, Input.gyro.rotationRate.z);
// disp_data += string.Format("Magneto data: x: {0:F4}, y: {1:F4}, z: {2:F4}.\n", Input.compass.rawVector.x, Input.compass.rawVector.y, Input.compass.rawVector.z);
// // Continuous angles data
// disp_data += string.Format("Angle about xyz GYRO:\n x: {0:F4}, y: {1:F4}, z: {2:F4}.\n", gyroIntegrator.orientationX, gyroIntegrator.orientationY, gyroIntegrator.orientationZ);
// disp_data += string.Format("Angle about xyz MAGNETOMETER:\n x: {0:F4}, y: {1:F4}, z: {2:F4}. \n", magnetoAxisRotationAngles.aboutX, magnetoAxisRotationAngles.aboutY, magnetoAxisRotationAngles.aboutZ);
// disp_data += string.Format("Gravity data: x: {0:F4}, y: {1:F4}, z: {2:F4}.\n", findGX.outputHistory[0], findGY.outputHistory[0], findGZ.outputHistory[0]);
// disp_data += string.Format("Kalman data: x: {0:F4}, y: {1:F4}, z: {2:F4}.\n", kalmanX.xhat, kalmanY.xhat, kalmanZ.xhat);
// Heading data
disp_data += string.Format("Heading at: {0:F4}, degrees, filteredHeading @{1:F4}\n", (float)(heading.current_heading * (180 / Math.PI)), (float)(filteredHeading.outputHistory[0] * (180 / Math.PI)));
// Update compass arrow
compass_arrow.Rotate(new Vector3(0, 0, (float)(heading.current_heading * (180 / Math.PI) - compass_arrow.eulerAngles.z)));
// Update text on the screen
stepCountViewer.GetComponent <UnityEngine.UI.Text>().text = disp_data;
// data logging data
// dl_data += "," + pedometer.dist_travelled_this_step + "," + heading.current_heading + ",";
dl_data += "," + pedometer.dist_travelled_this_step + "," + heading.current_heading + "," + filteredHeading.outputHistory[0] + "," + Input.gyro.rotationRate.z + "," + gyroIntegrator.orientationZ + "," + Input.compass.rawVector.z + "," + kalmanZ.xhat + ",";
dl.AppendData(dl_data);
}
}
/// <summary>
/// Logs the data from the round of calculations.
/// </summary>
/// <param name="time">Seconds since start of trial</param>
/// <param name="currentMotion">Current motion</param>
/// <param name="calculatedMotion">New motion, calculated by dynamics</param>
/// <param name="recording">The c</param>
/// <param name="controlInput">Current joystick position</param>
internal void LogData(double time, MotionCommand currentMotion, MotionCommand calculatedMotion, RecordingTimepoint recording, ControlInput controlInput)
{
StringBuilder builder;
builder = new StringBuilder();
if (Hulk.InnerAxis == Hulk.Axis.Roll)
{
builder.AppendFormat(
"{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}\n",
time,
recording.TrialNumber,
recording.TrialPhase,
recording.DirectionOfBalance.roll,
recording.DirectionOfBalance.pitch,
recording.DirectionOfBalance.yaw,
recording.MovingDirectionOfBalance.roll,
recording.MovingDirectionOfBalance.pitch,
recording.MovingDirectionOfBalance.yaw,
currentMotion.innerPosition.ToString("F6"),
currentMotion.outerPosition.ToString("F6"),
0,
currentMotion.innerVelocity.ToString("F6"),
currentMotion.outerVelocity.ToString("F6"),
0,
calculatedMotion.innerPosition.ToString("F6"),
calculatedMotion.outerPosition.ToString("F6"),
0,
(controlInput.trigger ? "1" : "0")
);
}
else
{
builder.AppendFormat(
"{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}\n",
time,
recording.TrialNumber,
recording.TrialPhase,
recording.DirectionOfBalance.roll,
recording.DirectionOfBalance.pitch,
recording.DirectionOfBalance.yaw,
recording.MovingDirectionOfBalance.roll,
recording.MovingDirectionOfBalance.pitch,
recording.MovingDirectionOfBalance.yaw,
0,
currentMotion.outerPosition.ToString("F6"),
currentMotion.innerPosition.ToString("F6"),
0,
currentMotion.outerVelocity.ToString("F6"),
currentMotion.innerVelocity.ToString("F6"),
0,
calculatedMotion.outerPosition.ToString("F6"),
calculatedMotion.innerPosition.ToString("F6"),
(controlInput.trigger ? "1" : "0")
);
}
DataLogger.AppendData(builder.ToString());
}
/// <summary>
/// Logs the data from the round of calculations.
/// </summary>
/// <param name="time">Seconds since start of trial</param>
/// <param name="trial">The trial being run</param>
/// <param name="currentMotion">Current motion</param>
/// <param name="calculatedMotion">New motion, calculated by dynamics</param>
/// <param name="controlInput">Current joystick position</param>
/// <param name="noise">Noise input to position</param>
internal void LogData(double time, Trial trial, MotionCommand currentMotion, MotionCommand calculatedMotion,
ControlInput controlInput, RotationAngles noise)
{
StringBuilder builder;
RotationAngles position;
RotationAngles calcPosition;
int phase;
position = new RotationAngles();
calcPosition = new RotationAngles();
builder = new StringBuilder();
switch (trial.TrialStatus)
{
case Trial.Status.Initializing:
phase = 0;
break;
case Trial.Status.Moving:
phase = 1;
if (Hulk.InnerAxis == Hulk.Axis.Roll)
{
position.roll = currentMotion.innerPosition;
}
else
{
position.yaw = currentMotion.innerPosition;
}
position.pitch = currentMotion.outerPosition;
break;
case Trial.Status.BalancingDOBChanging:
phase = 2;
if (Hulk.InnerAxis == Hulk.Axis.Roll)
{
position.roll = currentMotion.innerPosition + trial.MovingDirectionOfBalance.roll;
calcPosition.roll = calculatedMotion.innerPosition + trial.MovingDirectionOfBalance.roll;
}
else
{
position.yaw = currentMotion.innerPosition + trial.MovingDirectionOfBalance.yaw;
calcPosition.yaw = calculatedMotion.innerPosition + trial.MovingDirectionOfBalance.yaw;
}
position.pitch = currentMotion.outerPosition + trial.MovingDirectionOfBalance.pitch;
calcPosition.pitch = calculatedMotion.outerPosition + trial.MovingDirectionOfBalance.pitch;
break;
case Trial.Status.BalancingDOBStable:
phase = 3;
if (Hulk.InnerAxis == Hulk.Axis.Roll)
{
position.roll = currentMotion.innerPosition + trial.MovingDirectionOfBalance.roll;
calcPosition.roll = calculatedMotion.innerPosition + trial.MovingDirectionOfBalance.roll;
}
else
{
position.yaw = currentMotion.innerPosition + trial.MovingDirectionOfBalance.yaw;
calcPosition.yaw = calculatedMotion.innerPosition + trial.MovingDirectionOfBalance.yaw;
}
position.pitch = currentMotion.outerPosition + trial.MovingDirectionOfBalance.pitch;
calcPosition.pitch = calculatedMotion.outerPosition + trial.MovingDirectionOfBalance.pitch;
break;
case Trial.Status.Resetting:
phase = 4;
if (Hulk.InnerAxis == Hulk.Axis.Roll)
{
position.roll = currentMotion.innerPosition;
}
else
{
position.yaw = currentMotion.innerPosition;
}
position.pitch = currentMotion.outerPosition;
break;
case Trial.Status.Complete:
phase = 5;
break;
default:
phase = -1;
break;
}
if (Hulk.InnerAxis == Hulk.Axis.Roll)
{
builder.AppendFormat("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18},{19},{20},{21},{22},{23},{24},{25},{26},{27},{28},{29},{30}\n",
time,
trial.TrialNumber,
phase,
trial.DirectionOfBalance.roll,
trial.DirectionOfBalance.pitch,
trial.DirectionOfBalance.yaw,
trial.MovingDirectionOfBalance.roll,
trial.MovingDirectionOfBalance.pitch,
trial.MovingDirectionOfBalance.yaw,
position.roll,
position.pitch,
0,
currentMotion.innerVelocity.ToString("F6"),
currentMotion.outerVelocity.ToString("F6"),
0,
((calculatedMotion != null) ? calcPosition.roll.ToString("F6") : "0"),
((calculatedMotion != null) ? calcPosition.pitch.ToString("F6") : "0"),
0,
((calculatedMotion != null) ? calculatedMotion.innerVelocity.ToString("F6") : "0"),
((calculatedMotion != null) ? calculatedMotion.outerVelocity.ToString("F6") : "0"),
0,
((calculatedMotion != null) ? calculatedMotion.innerAcceleration.ToString("F6") : "0"),
((calculatedMotion != null) ? calculatedMotion.outerAcceleration.ToString("F6") : "0"),
0,
controlInput.x.ToString("F6"),
controlInput.y.ToString("F6"),
(controlInput.blanked ? "1" : "0"),
(controlInput.trigger ? "1" : "0"),
noise.roll,
noise.pitch,
noise.yaw
);
}
else
{
builder.AppendFormat("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18},{19},{20},{21},{22},{23},{24},{25},{26},{27},{28},{29},{30}\n",
time,
trial.TrialNumber,
phase,
trial.DirectionOfBalance.roll,
trial.DirectionOfBalance.pitch,
trial.DirectionOfBalance.yaw,
trial.MovingDirectionOfBalance.roll,
trial.MovingDirectionOfBalance.pitch,
trial.MovingDirectionOfBalance.yaw,
0,
position.pitch,
position.yaw,
0,
currentMotion.outerVelocity.ToString("F6"),
currentMotion.innerVelocity.ToString("F6"),
0,
((calculatedMotion != null) ? calcPosition.pitch.ToString("F6") : "0"),
((calculatedMotion != null) ? calcPosition.yaw.ToString("F6") : "0"),
0,
((calculatedMotion != null) ? calculatedMotion.outerVelocity.ToString("F6") : "0"),
((calculatedMotion != null) ? calculatedMotion.innerVelocity.ToString("F6") : "0"),
0,
((calculatedMotion != null) ? calculatedMotion.outerAcceleration.ToString("F6") : "0"),
((calculatedMotion != null) ? calculatedMotion.innerAcceleration.ToString("F6") : "0"),
controlInput.x.ToString("F6"),
controlInput.y.ToString("F6"),
(controlInput.blanked ? "1" : "0"),
(controlInput.trigger ? "1" : "0"),
noise.roll,
noise.pitch,
noise.yaw
);
}
DataLogger.AppendData(builder.ToString());
}
/// <summary>
/// Logs the data from the round of calculations.
/// </summary>
/// <param name="time">Seconds since start of trial</param>
/// <param name="currentMotion">Current motion</param>
/// <param name="controlInput">Current forcing function</param>
private void LogData(double time, MotionCommand currentMotion, MotionCommand calculatedMotion, ControlInput controlInput)
{
StringBuilder builder;
int phase;
builder = new StringBuilder();
phase = 0;
if (Hulk.InnerAxis == Hulk.Axis.Roll)
{
builder.AppendFormat(
"{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}\n",
time,
phase,
currentMotion.innerPosition.ToString("F6"),
currentMotion.outerPosition.ToString("F6"),
0,
currentMotion.innerVelocity.ToString("F6"),
currentMotion.outerVelocity.ToString("F6"),
0,
((calculatedMotion != null) ? calculatedMotion.innerPosition.ToString("F6") : "0"),
((calculatedMotion != null) ? calculatedMotion.outerPosition.ToString("F6") : "0"),
0,
((calculatedMotion != null) ? calculatedMotion.innerVelocity.ToString("F6") : "0"),
((calculatedMotion != null) ? calculatedMotion.outerVelocity.ToString("F6") : "0"),
0,
((calculatedMotion != null) ? calculatedMotion.innerAcceleration.ToString("F6") : "0"),
((calculatedMotion != null) ? calculatedMotion.outerAcceleration.ToString("F6") : "0"),
0,
controlInput.x.ToString("F6"),
controlInput.y.ToString("F6")
);
}
else
{
builder.AppendFormat(
"{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}\n",
time,
phase,
0,
currentMotion.outerPosition.ToString("F6"),
currentMotion.innerPosition.ToString("F6"),
0,
currentMotion.outerVelocity.ToString("F6"),
currentMotion.innerVelocity.ToString("F6"),
0,
((calculatedMotion != null) ? calculatedMotion.outerPosition.ToString("F6") : "0"),
((calculatedMotion != null) ? calculatedMotion.innerPosition.ToString("F6") : "0"),
0,
((calculatedMotion != null) ? calculatedMotion.outerVelocity.ToString("F6") : "0"),
((calculatedMotion != null) ? calculatedMotion.innerVelocity.ToString("F6") : "0"),
0,
((calculatedMotion != null) ? calculatedMotion.outerAcceleration.ToString("F6") : "0"),
((calculatedMotion != null) ? calculatedMotion.innerAcceleration.ToString("F6") : "0"),
controlInput.x.ToString("F6"),
controlInput.y.ToString("F6")
);
}
DataLogger.AppendData(builder.ToString());
}
