/// <summary>
/// Called from the main control loop whenever the task is running.
/// DO NOT call this method directly from DelayMeasurementTask or DelayMeasurementPanel.
/// </summary>
public override void ContinueTask()
{
ControlInput controlInput;
FunctionTimepoint function;
MotionCommand command;
double elapsedTime;
elapsedTime = runningStopwatch.ElapsedMilliseconds * 1.0 / 1000;
function = ForcingFunctions.GetForcingFunction(elapsedTime);
controlInput = (Hulk.InnerAxis == Hulk.Axis.Roll) ?
new ControlInput(function.Input.roll, function.Input.pitch, false, false) :
new ControlInput(function.Input.yaw, function.Input.pitch, false, false);
command = new MotionCommand();
command.innerVelocity = controlInput.x * amplitude;
command.outerVelocity = controlInput.y * amplitude;
command.innerAcceleration = 300;
command.outerAcceleration = 300;
Hulk.SetCommand(command);
Hulk.ContinueTask();
LogData(elapsedTime, Hulk.CurrentMotion, command, controlInput);
if (elapsedTime >= ForcingFunctions.GetForcingFunctionMaxTime())
{
Hulk.StopTask();
return;
}
}
/// <summary>
/// Called from the main control loop whenever the task is running.
/// DO NOT call this method directly from RotationTask or RotationPanel.
/// </summary>
public override void ContinueTask()
{
if ((stopTime != DateTime.MinValue) && (DateTime.Now.CompareTo(stopTime) >= 0))
{
// Duration limit reached.
Hulk.StopTask();
}
else
{
Hulk.ContinueTask();
}
}
/// <summary>
/// Called from the main control loop whenever the task is running.
/// DO NOT call this method directly from PositionTask or PositionPanel.
/// </summary>
public override void ContinueTask()
{
MotionCommand command;
ControlInput joystickInput;
// Continually command the chair to position itself according to joystick input
joystickInput = InputController.JoystickInput;
command = new MotionCommand {
outerPosition = startOuterPosition + (joystickInput.y * 90.0),
innerPosition = startInnerPosition + (joystickInput.x * 90.0),
};
Hulk.SetCommand(command);
Hulk.ContinueTask();
}
/// <summary>
/// Called from the main control loop whenever the task is running.
/// DO NOT call this method directly from PassiveMovementTask or PassiveMovementPanel.
/// </summary>
public override void ContinueTask()
{
MotionCommand command;
RecordingTimepoint recording;
double elapsedTime;
double angleDiff;
elapsedTime = runningStopwatch.ElapsedMilliseconds * 1.0 / 1000;
// Check whether the entire block is complete
if (Recordings.IsEndOfRecordingSeries(currentTrialNumber, elapsedTime))
{
// Play the last recording timepoint
recording = Recordings.GetRecording(currentTrialNumber, elapsedTime);
command = new MotionCommand();
command.outerPosition = recording.Angles.pitch;
if (Hulk.ChairMount == Hulk.MountType.Back)
{
command.innerPosition = recording.Angles.roll;
}
else
{
command.innerPosition = recording.Angles.yaw;
}
Hulk.SetCommand(command);
Hulk.ContinueTask();
// Stop the HULK and return to idling
Hulk.StopTask();
return;
}
// Check whether the current trial is complete
if (Recordings.IsEndOfRecordingTrial(currentTrialNumber, elapsedTime))
{
// Clean up from last trial
DataLogger.CloseDataLog();
runningStopwatch.Reset();
// Prepare for next trial
runningStopwatch.Start();
currentTrialNumber++;
numClicks = 0;
((PassiveMovementPanel)panel).UpdateListbox();
DataLogger.AcquireDataLog(String.Format("recording{0:000}.csv", currentTrialNumber), dataLogHeader);
return;
}
// Determine the control input for this simulation step
if (Recordings.HasRecording())
{
// Check whether the trigger has just been pressed
if (!previousTrigger && InputController.JoystickInput.trigger)
{
numClicks++;
}
previousTrigger = InputController.JoystickInput.trigger;
recording = Recordings.GetRecording(currentTrialNumber, elapsedTime);
command = new MotionCommand();
command.outerPosition = recording.Angles.pitch;
if (Hulk.ChairMount == Hulk.MountType.Back)
{
command.innerPosition = recording.Angles.roll;
}
else
{
command.innerPosition = recording.Angles.yaw;
}
// Stop if a large angle change is commanded. A large angle change could be dangerous at these accelerations.
angleDiff = Math.Abs(lastPositionInner - command.innerPosition);
if ((angleDiff > 3.0) && (angleDiff < 357.0))
{
logger.Warn("SAFETY: Instantaneous INNER move >3 deg prevented. Current=" +
lastPositionInner.ToString("F2") + " New=" + command.innerPosition.ToString("F2"));
Hulk.StopTask();
return;
}
angleDiff = Math.Abs(lastPositionOuter - command.outerPosition);
if ((angleDiff > 3.0) && (angleDiff < 357.0))
{
logger.Warn("SAFETY: Instantaneous OUTER move >3 deg prevented. Current=" +
lastPositionOuter.ToString("F2") + " New=" + command.outerPosition.ToString("F2"));
Hulk.StopTask();
return;
}
lastPositionOuter = command.outerPosition;
lastPositionInner = command.innerPosition;
LogData(elapsedTime, Hulk.CurrentMotion, command, recording, InputController.JoystickInput);
Hulk.SetCommand(command);
Hulk.ContinueTask();
}
}
/// <summary>
/// Called by the control loop when the participant is actively balancing.
/// </summary>
private void HandleStateBalancing()
{
ControlInput controlInput;
MotionCommand calculatedMotion;
MotionCommand newMotion;
RotationAngles noise;
double time;
double dt;
if (queueResetEndSound)
{
trial.PlayResetEndSound();
queueResetEndSound = false;
}
// Calculate how much time elapsed since the previous call
time = trialStopwatch.ElapsedMilliseconds;
dt = (time - previousMillis) / 1000.0;
// Remember the time for the next call
previousMillis = time;
// Move the DOB slowly towards its value for the current trial
if (trial.TrialStatus == Trial.Status.BalancingDOBChanging)
{
UpdateMovingDOB();
}
if ((trial.TrialStatus == Trial.Status.BalancingDOBStable) && (trialStopwatch.ElapsedMilliseconds / 1000.0) >= trial.TimeLimit)
{
// Trial time limit was reached
trial.TrialStatus = Trial.Status.Complete;
}
else
{
// Trial time limit has not been reached. Keep balancing.
controlInput = GetInput();
CheckJoystickPresses(controlInput);
// Determine new motion based on current position and joystick input
MotionCommand lastKnownHulkPosition = Hulk.CurrentMotion;
newMotion = new MotionCommand();
// Determine the angle between the current location and the desired location
if (Hulk.InnerAxis == Hulk.Axis.Roll)
{
newMotion.innerPosition = lastKnownHulkPosition.innerPosition - trial.MovingDirectionOfBalance.roll;
}
else
{
newMotion.innerPosition = lastKnownHulkPosition.innerPosition - trial.MovingDirectionOfBalance.yaw;
}
newMotion.outerPosition = lastKnownHulkPosition.outerPosition - trial.MovingDirectionOfBalance.pitch;
// Keep the angles between -180 and 180
if (newMotion.innerPosition > 180.0)
{
newMotion.innerPosition = newMotion.innerPosition - 360.0;
}
else if (newMotion.innerPosition < -180.0)
{
newMotion.innerPosition = newMotion.innerPosition + 360.0;
}
if (newMotion.outerPosition > 180.0)
{
newMotion.outerPosition = newMotion.outerPosition - 360.0;
}
else if (newMotion.outerPosition < -180.0)
{
newMotion.outerPosition = newMotion.outerPosition + 360.0;
}
// Get the current velocity
newMotion.innerVelocity = lastKnownHulkPosition.innerVelocity;
newMotion.outerVelocity = lastKnownHulkPosition.outerVelocity;
// Update new motion based on IVP dynamics
noise = trial.GetNextNoise();
calculatedMotion = Dynamics.CalculateDynamics(trial, newMotion, controlInput, noise, dt);
LogData(logStopwatch.ElapsedMilliseconds / 1000.0, trial, newMotion, calculatedMotion, controlInput, noise);
// Check whether the max angle was exceeded
if (trial.ExceedsMaxAngle(calculatedMotion))
{
// Participant went too far away from the balance point.
trial.TrialStatus = Trial.Status.Resetting;
}
else
{
// Participant is still balancing
// Maximize ability of motors to follow the commanded velocities
calculatedMotion.outerAcceleration = trial.MaxAcceleration;
calculatedMotion.innerAcceleration = trial.MaxAcceleration;
calculatedMotion.outerPosition = Hulk.ANY_MOTION;
calculatedMotion.innerPosition = Hulk.ANY_MOTION;
// Cancel movement commands for unused axes
if ((Hulk.InnerAxis == Hulk.Axis.Roll) && !trial.UseRoll)
{
calculatedMotion.innerVelocity = 0.0;
}
else if ((Hulk.InnerAxis == Hulk.Axis.Yaw) && !trial.UseYaw)
{
calculatedMotion.innerVelocity = 0.0;
}
if (!trial.UsePitch)
{
calculatedMotion.outerVelocity = 0.0;
}
Hulk.SetCommand(calculatedMotion);
Hulk.ContinueTask();
}
}
}
/// <summary>
/// Called from the main control loop whenever the task is running.
/// DO NOT call this method directly from UnrealLandscapesDemoTask or RotationPanel.
/// </summary>
public override void ContinueTask()
{
Hulk.ContinueTask();
_transmitStatusToNetworkClients();
}
