/// <summary>
/// Helper function for the RK4 solver.
/// </summary>
private static AxisMotionDerivative RK4Evaluate(Trial trial, AxisMotionState initial, double dt,
AxisMotionDerivative d, ControlInput controlInput, AccelerationFunction accelFunction)
{
AxisMotionState state;
state = new AxisMotionState(initial.position + d.dAngle * dt, initial.velocity + d.dVelocity * dt,
initial.acceleration);
return(new AxisMotionDerivative(state.velocity, accelFunction(trial, state, controlInput)));
}
/// <summary>
/// Computes the new acceleration for a given axis.
/// This function can be modified to create a new dynamics model.
/// </summary>
/// <param name="trial">The current trial</param>
/// <param name="state">The current motion of an axis</param>
/// <param name="controlInput">The current joystick or forcing function input</param>
/// <returns>The acceleration</returns>
private static double ComputeAcceleration(Trial trial, AxisMotionState state, ControlInput controlInput)
{
if (controlMode == ControlMode.Acceleration)
{
return(trial.AccelerationConstant * Math.Sin(state.position * Math.PI / 180.0) -
(controlInput.x * trial.JoystickGain));
}
else
{
return(trial.AccelerationConstant * Math.Sin(state.position * Math.PI / 180.0));
}
}
/// <summary>
/// Starts the task. Called when the operator clicks the Go button.
/// </summary>
/// <param name="stateInfo">Not used. Here so that the method can be called by a worker thread.</param>
internal void Go(Object stateInfo)
{
MotionCommand moveCommand;
logger.Debug("Enter: Go(Object)");
Trials.CurrentTrialIndex = 0;
trial = Trials.CurrentTrial;
SendPlottingAxesCenterChange();
queueResetEndSound = false;
StartLogging();
// Begin moving to the starting location of the first trial
logger.Info("Moving to location for beginning of balance trial: " + trial.TrialNumber);
trial.MovingDirectionOfBalance.yaw = trial.DirectionOfBalance.yaw;
trial.MovingDirectionOfBalance.pitch = trial.DirectionOfBalance.pitch;
trial.MovingDirectionOfBalance.roll = trial.DirectionOfBalance.roll;
trial.TrialStatus = Trial.Status.Moving;
trial.PlayMoveSound();
moveCommand = new MotionCommand {
innerVelocity = Hulk.NORMAL_VELOCITY,
outerVelocity = Hulk.NORMAL_VELOCITY,
innerAcceleration = Hulk.NORMAL_ACCELERATION,
outerAcceleration = Hulk.NORMAL_ACCELERATION
};
if (trial.BeginAt == null)
{
moveCommand.innerPosition = (Hulk.InnerAxis == Hulk.Axis.Roll) ?
trial.DirectionOfBalance.roll : trial.DirectionOfBalance.yaw;
moveCommand.outerPosition = trial.DirectionOfBalance.pitch;
}
else
{
moveCommand.innerPosition = (Hulk.InnerAxis == Hulk.Axis.Roll) ?
trial.BeginAt.roll : trial.BeginAt.yaw;
moveCommand.outerPosition = trial.BeginAt.pitch;
}
Hulk.SetCommandType(Hulk.CommandType.ModulusPosition);
Hulk.SetCommand(moveCommand);
Hulk.StartDefinedMove(true);
}
/// <summary>
/// Called by th control loop when the trial's time limit has been reached.
/// </summary>
private void HandleStateComplete()
{
logger.Info("Completed balance trial: " + trial.TrialNumber);
DataLogger.CloseDataLog();
bool advanceToNextTrial = true;
if ((trial.NumberIndications == 0) && trial.JoystickIndicationsMandatory)
{
logger.Warn("Participant did not indicate during this trial - restarting trial");
StartLogging();
StartTrial();
advanceToNextTrial = false;
}
if (advanceToNextTrial)
{
trial.PlayEndSound();
if (Trials.CurrentTrialIndex == (Trials.List.Count - 1))
{
// Entire protocol has been completed
Hulk.StopTask();
}
else
{
// Continue to following trial
Trials.CurrentTrialIndex++;
trial = Trials.CurrentTrial;
StartLogging();
trial.MovingDirectionOfBalance.roll = Trials.PreviousTrial.MovingDirectionOfBalance.roll;
trial.MovingDirectionOfBalance.pitch = Trials.PreviousTrial.MovingDirectionOfBalance.pitch;
trial.MovingDirectionOfBalance.yaw = Trials.PreviousTrial.MovingDirectionOfBalance.yaw;
StartTrial();
}
}
}
/// <summary>
/// RK4 solver.
/// </summary>
/// <param name="trial">The current trial</param>
/// <param name="state">State of the system (input / output)</param>
/// <param name="dt">Time since last evaluation</param>
/// <param name="accelFunction">Function to calculate acceleration</param>
/// <param name="controlInput">The current joystick or forcing function input</param>
private static void Integrate(Trial trial, AxisMotionState state, double dt, AccelerationFunction accelFunction,
ControlInput controlInput)
{
AxisMotionDerivative a;
AxisMotionDerivative b;
AxisMotionDerivative c;
AxisMotionDerivative d;
double dadt;
double dvdt;
a = new AxisMotionDerivative(state.velocity, accelFunction(trial, state, controlInput));
b = RK4Evaluate(trial, state, dt * 0.5, a, controlInput, accelFunction);
c = RK4Evaluate(trial, state, dt * 0.5, b, controlInput, accelFunction);
d = RK4Evaluate(trial, state, dt, c, controlInput, accelFunction);
dadt = 1.0 / 6.0 * (a.dAngle + 2.0 * (b.dAngle + c.dAngle) + d.dAngle);
dvdt = 1.0 / 6.0 * (a.dVelocity + 2.0 * (b.dVelocity + c.dVelocity) + d.dVelocity);
state.position += dadt * dt;
state.velocity += dvdt * dt;
state.acceleration = dvdt;
}
/// <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);
}
/// <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());
}
