/// <summary>
/// Creates the recording timepoint from a single line of a CSV file.
/// </summary>
/// <param name="line">The CSV file line</param>
internal RecordingTimepoint(String line)
{
String[] param;
angles = new RotationAngles();
directionOfBalance = new RotationAngles();
movingDirectionOfBalance = new RotationAngles();
param = line.Split(',');
if (param.Length < 12)
{
throw new IOException("Incorrect number of values in recording timepoint string.");
}
try {
Double.TryParse(param[0], out time);
Int32.TryParse(param[1], out trialNumber);
Int32.TryParse(param[2], out trialPhase);
Double.TryParse(param[3], out directionOfBalance.roll);
Double.TryParse(param[4], out directionOfBalance.pitch);
Double.TryParse(param[5], out directionOfBalance.yaw);
Double.TryParse(param[6], out movingDirectionOfBalance.roll);
Double.TryParse(param[7], out movingDirectionOfBalance.pitch);
Double.TryParse(param[8], out movingDirectionOfBalance.yaw);
Double.TryParse(param[9], out angles.roll);
Double.TryParse(param[10], out angles.pitch);
Double.TryParse(param[11], out angles.yaw);
} catch (Exception) {
throw new IOException("Parsing failed for recording timepoint string.");
}
}
internal bool SetNoiseProfile()
{
RotationAngles angles;
StreamReader reader;
String[] fragments;
String line;
logger.Debug("Enter: SetNoiseProfile()");
if ((noiseProfile == "") || (noiseProfile == null) || (noiseProfile.ToUpper() == "NULL") || (noiseProfile.ToUpper() == "NONE"))
{
return(true);
}
if (!Directory.Exists(ConfigurationManager.AppSettings["NoiseDirectory"]))
{
logger.Error("[Trial] Could not load noise profile '" + noiseProfile + "' because the noise directory does not exist: " + ConfigurationManager.AppSettings["NoiseDirectory"]);
return(false);
}
try
{
reader = new StreamReader(ConfigurationManager.AppSettings["NoiseDirectory"] + "\\" + noiseProfile);
noisePoints = new List <RotationAngles>();
while ((line = reader.ReadLine()) != null)
{
// Ignore comment lines
if (line.StartsWith(";"))
{
continue;
}
// Parse the line for a set of noise values
fragments = line.Split(',');
angles = new RotationAngles();
angles.roll = Double.Parse(fragments[0]) * noiseAmplitude;
angles.pitch = Double.Parse(fragments[1]) * noiseAmplitude;
angles.yaw = Double.Parse(fragments[2]) * noiseAmplitude;
noisePoints.Add(angles);
}
}
catch (Exception e)
{
logger.Error("[Trial] Could not load noise profile '" + ConfigurationManager.AppSettings["NoiseDirectory"] + "\\" + noiseProfile + "'. Reason: " + e.Message);
return(false);
}
return(true);
}
public RealTime()
{
InitializeComponent();
angles = new RotationAngles(-1, -1, -1);
angles.gyroXNoiseCorrect = -4.84 * .02;
angles.gyroYNoiseCorrect = 2.35 * .02;
angles.gyroZNoiseCorrect = -1.03 * .02;
listView.ItemsSource = output;
StartAccelAndGyroSensors();
Close.Clicked += Close_Clicked;
}
/// <summary>
/// Creates the function timepoint from a single line of a CSV file.
/// </summary>
/// <param name="line">The CSV file line</param>
internal FunctionTimepoint(String line)
{
String[] param;
input = new RotationAngles();
param = line.Split(',');
if (param.Length != 4)
{
throw new IOException("Incorrect number of values in function timepoint input string.");
}
try {
Double.TryParse(param[0], out time);
Double.TryParse(param[1], out input.yaw);
Double.TryParse(param[2], out input.pitch);
Double.TryParse(param[3], out input.roll);
} catch (Exception) {
throw new IOException("Parsing failed for function timepoint input string.");
}
}
/// <summary>
/// Calculates the next motion, based on the current system state.
/// </summary>
/// <param name="trial">Trial parameters</param>
/// <param name="currentState">Position, velocity, etc of the system</param>
/// <param name="originalControlInput">Current joystick input</param>
/// <param name="noise">Noise values to add to position before calculations</param>
/// <param name="dt">Time elapsed since last calculation</param>
/// <returns>The new position, velocity, etc of the system</returns>
public static MotionCommand CalculateDynamics(Trial trial, MotionCommand currentState, ControlInput originalControlInput,
RotationAngles noise, double dt)
{
AxisMotionState axis1State;
AxisMotionState axis2State;
ControlInput controlInput;
MotionCommand newMotionCommand;
// Convert to structs used in dynamics code
axis1State = new AxisMotionState(currentState.outerPosition, currentState.outerVelocity, 0);
axis2State = new AxisMotionState(currentState.innerPosition, currentState.innerVelocity, 0);
if (originalControlInput.blanked)
{
controlInput = new ControlInput(0, 0, false, false);
}
else
{
controlInput = new ControlInput(originalControlInput.x, originalControlInput.y, false, false);
}
// Handle joystick input + noise
// figure out what the noise for each axis is
double axis1Noise = noise.pitch;
double axis2Noise = (Hulk.InnerAxis == Hulk.Axis.Roll) ? noise.roll : noise.yaw;
// figure out how joystick input should affect each axis
double axis1Delta = controlInput.y * trial.JoystickGain;
double axis2Delta = controlInput.x * trial.JoystickGain;
// uncomment for debugging purpose
// System.Diagnostics.Debug.Write("Axis 1 noise: " + axis1Noise + ", joystick delta: " + axis1Delta + ", total delta: " + (axis1Delta + axis1Noise));
// System.Diagnostics.Debug.WriteLine(", Axis 2 noise: " + axis2Noise + ", joystick delta: " + axis2Delta + ", total delta: " + (axis2Delta + axis2Noise));
// Apply joystick input + noise
switch (controlMode)
{
case ControlMode.Velocity:
axis1State.velocity -= axis1Delta + axis1Noise;
axis2State.velocity -= axis2Delta + axis2Noise;
break;
case ControlMode.Position:
axis1State.position -= axis1Delta + axis1Noise;
axis2State.position -= axis2Delta + axis2Noise;
break;
}
// Integrate to obtain new motion
Integrate(trial, axis1State, dt, ComputeAcceleration, controlInput);
Integrate(trial, axis2State, dt, ComputeAcceleration, controlInput);
// Upper limit on velocity for safety
axis1State.velocity = Math.Min(axis1State.velocity, trial.MaxVelocity);
axis1State.velocity = Math.Max(axis1State.velocity, -trial.MaxVelocity);
axis2State.velocity = Math.Min(axis2State.velocity, trial.MaxVelocity);
axis2State.velocity = Math.Max(axis2State.velocity, -trial.MaxVelocity);
// Lower limit on velocity to match precision of motion controller
int signInner = (Hulk.InnerAxis == Hulk.Axis.Roll) ? Math.Sign(axis1State.position - trial.MovingDirectionOfBalance.roll) : Math.Sign(axis1State.position - trial.MovingDirectionOfBalance.yaw);
int signOuter = Math.Sign(axis2State.position - trial.MovingDirectionOfBalance.pitch);
if (Math.Abs(axis1State.velocity) < 0.5)
{
axis1State.velocity = 0.5 * signInner;
}
if (Math.Abs(axis2State.velocity) < 0.5)
{
axis2State.velocity = 0.5 * signOuter;
}
// old code, in case we need to revert back
//if (axis1State.velocity > 0.0) {
// axis1State.velocity = Math.Max(axis1State.velocity, 0.5);
//} else {
// axis1State.velocity = Math.Min(axis1State.velocity, -0.5);
//}
//if (axis2State.velocity > 0.0) {
// axis2State.velocity = Math.Max(axis2State.velocity, 0.5);
//} else {
// axis2State.velocity = Math.Min(axis2State.velocity, -0.5);
//}
// Convert back to structs used in rest of code
newMotionCommand = new MotionCommand {
innerPosition = axis2State.position,
innerVelocity = axis2State.velocity,
innerAcceleration = Math.Abs(axis2State.acceleration),
outerPosition = axis1State.position,
outerVelocity = axis1State.velocity,
outerAcceleration = Math.Abs(axis1State.acceleration)
};
return(newMotionCommand);
}
void LateUpdate()
{
// Early out if we don't have a target
if (!target)
{
return;
}
var rigidbody = target.GetComponent <Rigidbody>();
if (rigidbody)
{
distance = Mathf.Lerp(distance, Mathf.Lerp(minDistance, maxDistance, Mathf.Clamp01((rigidbody.velocity.magnitude - minSpeed) / (maxSpeed - minSpeed))), Time.deltaTime);
}
float currentRotationAngle = transform.eulerAngles.y;
float currentHeight = transform.position.y;
// Calculate the current rotation angles
float wantedHeight = target.position.y + height;
RaycastHit hit;
int i = 0;
float wantedRotationAngle = RotationAngles.FirstOrDefault(
angle =>
{
var pos = PositionFromRotationAngle(angle, wantedHeight);
var direction = (target.position - pos).normalized;
pos -= direction * 2;
//Debug.DrawRay(pos - direction * 5, direction, Color.green);
if (Physics.Raycast(pos, direction, out hit))
{
//Debug.LogFormat("{0} ({1})", hit.collider.name, i++);
return(hit.collider.transform.IsChildOf(target));
}
return(false);
}
);
if (wantedRotationAngle == default(float))
{
wantedRotationAngle = RotationAngles.First();
}
//Debug.Log(wantedRotationAngle);
// Damp the rotation around the y-axis
currentRotationAngle = Mathf.LerpAngle(currentRotationAngle,
wantedRotationAngle,
rotationDamping * Time.deltaTime);
// Damp the height
currentHeight = Mathf.Lerp(currentHeight,
wantedHeight,
heightDamping * Time.deltaTime);
// Convert the angle into a rotation around the y-axis
Quaternion currentRotation = Quaternion.Euler(0, currentRotationAngle, 0);
// Set the position of the camera on the x-z plane to:
// distance meters behind the target
transform.position = target.position;
transform.position -= currentRotation * Vector3.forward * distance;
// Set the height of the camera
transform.position = new Vector3(transform.position.x,
currentHeight,
transform.position.z);
// Always look at the target
transform.LookAt(target);
}
/// <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>
/// Creates the trial from the row/column data contained in the csvreader
/// </summary>
/// <param name="csvReader">The csvreader containing the row and column data</param>
/// <param name="row">The row number of the data to use for this trial</param>
internal Trial(CsvReader csvReader, int row)
{
logger.Debug("Create: Trial(CsvReader, int)");
status = Status.Initializing;
directionOfBalance = new RotationAngles();
movingDirectionOfBalance = new RotationAngles();
visualOrientationOffset = new RotationAngles();
beginAtPoint = new RotationAngles();
reminderGiven = false;
trialNumber = csvReader.getValueAsInt("TrialNumber", row);
condition = csvReader.getValueAsString("Condition", row);
directionOfBalance.roll = csvReader.getValueAsDouble("DOBRoll", row);
directionOfBalance.pitch = csvReader.getValueAsDouble("DOBPitch", row);
directionOfBalance.yaw = csvReader.getValueAsDouble("DOBYaw", row);
accelerationConstant = csvReader.getValueAsDouble("AccelConstant", row);
maxAcceleration = csvReader.getValueAsDouble("MaxAcceleration", row);
maxVelocity = csvReader.getValueAsDouble("MaxVelocity", row);
maxAngle = csvReader.getValueAsDouble("MaxAngle", row);
maxAcceleration = csvReader.getValueAsDouble("MaxAcceleration", row);
restartWhenMaxAngle = csvReader.getValueAsBool("RestartWhenMaxAngle", row);
timeLimit = csvReader.getValueAsDouble("TimeLimit", row);
joystickGain = csvReader.getValueAsDouble("JoystickGain", row);
restartDOBOffsetMaxYaw = csvReader.getValueAsDouble("RestartDOBOffsetMaxYaw", row);
restartDOBOffsetMinYaw = csvReader.getValueAsDouble("RestartDOBOffsetMinYaw", row);
restartDOBOffsetMaxPitch = csvReader.getValueAsDouble("RestartDOBOffsetMaxPitch", row);
restartDOBOffsetMinPitch = csvReader.getValueAsDouble("RestartDOBOffsetMinPitch", row);
restartDOBOffsetMaxRoll = csvReader.getValueAsDouble("RestartDOBOffsetMaxRoll", row);
restartDOBOffsetMinRoll = csvReader.getValueAsDouble("RestartDOBOffsetMinRoll", row);
string val = csvReader.getValueAsString("JoystickControl", row);
if (val.ToUpper() == "MIRROR")
{
joystickControl = JoystickControl.Mirrored;
}
else if (val.ToUpper() == "CW")
{
joystickControl = JoystickControl.Clockwise;
}
else if (val.ToUpper() == "CCW")
{
joystickControl = JoystickControl.Counterclockwise;
}
else
{
joystickControl = JoystickControl.Normal;
}
noiseProfile = csvReader.getValueAsString("NoiseProfile", row);
noiseAmplitude = csvReader.getValueAsDouble("NoiseAmplitude", row);
useRoll = csvReader.getValueAsBool("UseRoll", row);
usePitch = csvReader.getValueAsBool("UsePitch", row);
useYaw = csvReader.getValueAsBool("UseYaw", row);
joystickIndicationsMandatory = csvReader.getValueAsBool("JoystickIndicationsMandatory", row);
bool useBeginPoint = false;
if (csvReader.hasValueAsDouble("BeginAtPitch", row) &&
csvReader.hasValueAsDouble("BeginAtYaw", row) &&
csvReader.hasValueAsDouble("BeginAtRoll", row))
{
beginAtPoint.yaw = csvReader.getValueAsDouble("BeginAtYaw", row);
beginAtPoint.pitch = csvReader.getValueAsDouble("BeginAtPitch", row);
beginAtPoint.roll = csvReader.getValueAsDouble("BeginAtRoll", row);
useBeginPoint = true;
}
if (!useBeginPoint)
{
beginAtPoint = null;
}
visualOrientationOffset.yaw = csvReader.getValueAsDouble("VisualYawOffset", row);
visualOrientationOffset.pitch = csvReader.getValueAsDouble("VisualPitchOffset", row);
visualOrientationOffset.roll = csvReader.getValueAsDouble("VisualRollOffset", row);
moveSound = csvReader.getValueAsString("MoveSound", row);
startSound = csvReader.getValueAsString("TrialStartSound", row);
endSound = csvReader.getValueAsString("TrialEndSound", row);
resetStartSound = csvReader.getValueAsString("ResetStartSound", row);
resetEndSound = csvReader.getValueAsString("ResetEndSound", row);
reminderSound = csvReader.getValueAsString("ReminderSound", row);
// sanity check
if (restartDOBOffsetMaxYaw > maxAngle)
{
throw new Exception("Error when reading row " + (row + 1) + " from file " + csvReader.FileName + "\r\n" + "Reason: the maximum DOB yaw offset when restarting (column: RestartDOBOffsetMaxYaw) is larger than the maximum angle (column: MaxAngle)");
}
if (restartDOBOffsetMaxPitch > maxAngle)
{
throw new Exception("Error when reading row " + (row + 1) + " from file " + csvReader.FileName + "\r\n" + "Reason: the maximum DOB pitch offset when restarting (column: RestartDOBOffsetMaxPitch) is larger than the maximum angle (column: MaxAngle)");
}
if (restartDOBOffsetMaxRoll > maxAngle)
{
throw new Exception("Error when reading row " + (row + 1) + " from file " + csvReader.FileName + "\r\n" + "Reason: the maximum DOB roll offset when restarting (column: RestartDOBOffsetMaxRoll) is larger than the maximum angle (column: MaxAngle)");
}
// SetNoiseProfile();
SetSoundPlayers();
}
/// <summary>
/// Creates an empty recording timepoint.
/// </summary>
internal RecordingTimepoint()
{
angles = new RotationAngles();
directionOfBalance = new RotationAngles();
movingDirectionOfBalance = new RotationAngles();
}
/// <summary>
/// Creates an empty function timepoint.
/// </summary>
internal FunctionTimepoint()
{
input = new RotationAngles();
}
