/// <summary> /// Starts the task. Called when the operator clicks the Go button. /// </summary> /// <param name="command">The movement command (velocity, acceleration)</param> /// <param name="duration">The duration of movement</param> internal void Go(MotionCommand command, double duration) { logger.Debug("Enter: Go(MotionCommand, double)"); if (command.innerPosition == Hulk.ANY_MOTION) { // Moving with given velocity/acceleration and (optionally) duration. logger.Info("Starting rotation task with given velocity, acceleration, and duration."); Hulk.SetCommandType(Hulk.CommandType.Velocity); if (duration == -1.0) { stopTime = DateTime.MinValue; } else { stopTime = DateTime.Now.AddSeconds(duration); } Hulk.SetCommand(command); Hulk.StartTask(); } else { // Moving to the given position. logger.Info("Starting rotation task to given position."); Hulk.SetCommandType(Hulk.CommandType.ModulusPosition); Hulk.SetCommand(command); Hulk.StartDefinedMove(false); } }
/// <summary> /// Starts the task. Called when the operator clicks the Go button. /// </summary> /// <param name="sender">Not used</param> /// <param name="e">Not used</param> internal void Go() { MotionCommand newMotionCommand; logger.Debug("Enter: Go()"); Hulk.Halt(); startOuterPosition = Hulk.CurrentMotion.outerPosition; startInnerPosition = Hulk.CurrentMotion.innerPosition; logger.Info("Starting position task with center at OUTER=" + startOuterPosition.ToString("F1") + " INNER=" + startInnerPosition.ToString("F1") + "."); Hulk.SetCommandType(Hulk.CommandType.ModulusPosition); newMotionCommand = new MotionCommand { outerVelocity = POSITIONING_VELOCITY, innerVelocity = POSITIONING_VELOCITY, outerAcceleration = POSITIONING_ACCELERATION, innerAcceleration = POSITIONING_ACCELERATION }; Hulk.SetCommand(newMotionCommand); Hulk.StartTask(); }
/// <summary> /// Gradually stops any rotation of the HULK, using the currently specified /// deceleration parameters. Called when the operator clicks the Soft Stop button. /// </summary> /// <param name="stateInfo">Not used. Here so that the method can be called by a worker thread.</param> internal void SoftStop(Object stateInfo) { MotionCommand newCommand; logger.Debug("Enter: SoftStop(Object)"); logger.Info("Decelerating."); Hulk.SetCommandType(Hulk.CommandType.Velocity); newCommand = new MotionCommand(); newCommand.innerVelocity = 0; newCommand.outerVelocity = 0; newCommand.innerAcceleration = Hulk.NORMAL_ACCELERATION; newCommand.outerAcceleration = Hulk.NORMAL_ACCELERATION; Hulk.SetCommand(newCommand); while ((Hulk.CurrentMotion.innerVelocity > 0.1) || (Hulk.CurrentMotion.outerVelocity > 0.1)) { Thread.Sleep(100); } Hulk.StopTask(); }
/// <summary> /// Starts the task. Called when the operator clicks the Go button. /// </summary> internal void Go() { MotionCommand newMotionCommand; logger.Debug("Enter: Go()"); logger.Info("Starting passive movement task."); previousTrigger = false; numClicks = 0; lastPositionOuter = Hulk.CurrentMotion.outerPosition; lastPositionInner = Hulk.CurrentMotion.innerPosition; currentTrialNumber = Recordings.GetFirstTrialNumber(); DataLogger.AcquireDataLog(String.Format("recording{0:000}.csv", currentTrialNumber), dataLogHeader); runningStopwatch.Reset(); runningStopwatch.Start(); Hulk.SetCommandType(Hulk.CommandType.ModulusPosition); newMotionCommand = new MotionCommand { outerVelocity = POSITIONING_VELOCITY, innerVelocity = POSITIONING_VELOCITY, outerAcceleration = POSITIONING_ACCELERATION, innerAcceleration = POSITIONING_ACCELERATION }; Hulk.SetCommand(newMotionCommand); Hulk.StartTask(); }
/// <summary> /// Called by the control loop when the participant exceeds the max allowable angle from the DOB. /// </summary> private void HandleResetting() { MotionCommand moveCommand; // Restart with balancing once reset is complete trial.TrialStatus = Trial.Status.Moving; Hulk.Halt(); trial.PlayResetStartSound(); queueResetEndSound = true; // Move back to the DOB moveCommand = new MotionCommand { innerVelocity = Hulk.NORMAL_VELOCITY, outerVelocity = Hulk.NORMAL_VELOCITY, innerAcceleration = Hulk.NORMAL_ACCELERATION, outerAcceleration = Hulk.NORMAL_ACCELERATION }; // calculate random offset from DOB to return the chair to double dobOffsetY = _calculateDOBOffset(trial.RestartDOBOffsetMinYaw, trial.RestartDOBOffsetMaxYaw); double dobOffsetP = _calculateDOBOffset(trial.RestartDOBOffsetMinPitch, trial.RestartDOBOffsetMaxPitch); double dobOffsetR = _calculateDOBOffset(trial.RestartDOBOffsetMinRoll, trial.RestartDOBOffsetMaxRoll); if (Math.Abs(dobOffsetY) > trial.MaxAngle) { logger.Warn("The calculated random offset for the DOB yaw return position is outside of the maximum angle specified for this trial. Using a random offset of 0!"); dobOffsetY = 0; } if (Math.Abs(dobOffsetP) > trial.MaxAngle) { logger.Warn("The calculated random offset for the DOB pitch return position is outside of the maximum angle specified for this trial. Using a random offset of 0!"); dobOffsetP = 0; } if (Math.Abs(dobOffsetR) > trial.MaxAngle) { logger.Warn("The calculated random offset for the DOB roll return position is outside of the maximum angle specified for this trial. Using a random offset of 0!"); dobOffsetR = 0; } double resetDobY = trial.MovingDirectionOfBalance.yaw + dobOffsetY; double resetDobP = trial.MovingDirectionOfBalance.pitch + dobOffsetP; double resetDobR = trial.MovingDirectionOfBalance.roll + dobOffsetR; moveCommand.innerPosition = (Hulk.InnerAxis == Hulk.Axis.Roll) ? resetDobR : resetDobY; moveCommand.outerPosition = resetDobP; Hulk.SetCommandType(Hulk.CommandType.ModulusPosition); Hulk.SetCommand(moveCommand); Hulk.StartDefinedMove(true); logger.Info("Participant lost control. Resetting to orientation: yaw " + resetDobY.ToString("0.##") + " pitch " + resetDobP.ToString("0.##") + " roll " + resetDobR.ToString("0.##")); _sendCommandStopTrial(); }
/// <summary> /// Starts the task. Called when the operator clicks the Go button. /// </summary> /// <param name="command">The movement command (velocity, acceleration)</param> /// <param name="duration">The duration of movement</param> internal void Go(MotionCommand command, double duration) { logger.Debug("Enter: Go(MotionCommand, double)"); if (command.innerPosition == Hulk.ANY_MOTION) { // Moving with given velocity/acceleration and (optionally) duration. logger.Info("Starting UnrealLandscapesDemoTask with given duration."); Hulk.SetCommandType(Hulk.CommandType.Velocity); Hulk.SetCommand(command); Hulk.StartTask(); } else { // Moving to the given position. logger.Info("Starting UnrealLandscapesDemoTask to given position."); Hulk.SetCommandType(Hulk.CommandType.ModulusPosition); Hulk.SetCommand(command); Hulk.StartDefinedMove(false); } }
/// <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> /// 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 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> /// Starts the task. Called when the operator clicks the Go button. /// </summary> /// <param name="functionFilename">The filename of the CSV file containing the forcing function</param> /// <param name="selectedAmplitude">The amplitude of the forcing function</param> /// <param name="selectedOffset">???</param> internal void Go(string functionFilename, double selectedAmplitude, double selectedOffset) { MotionCommand newMotionCommand; logger.Debug("Enter: Go()"); logger.Info("Starting delay measurement task."); amplitude = selectedAmplitude; // Move to the start location Hulk.SetCommandType(Hulk.CommandType.ModulusPosition); newMotionCommand = new MotionCommand(); newMotionCommand.innerPosition = selectedOffset; newMotionCommand.outerPosition = 0.0; newMotionCommand.outerVelocity = Hulk.NORMAL_VELOCITY; newMotionCommand.innerVelocity = Hulk.NORMAL_VELOCITY; newMotionCommand.outerAcceleration = Hulk.NORMAL_ACCELERATION; newMotionCommand.innerAcceleration = Hulk.NORMAL_ACCELERATION; Hulk.SetCommand(newMotionCommand); Hulk.StartDefinedMove(false); while (Hulk.SystemStatus != Hulk.Status.Idling) { Thread.Sleep(100); } // Prepare for trial DataLogger.AcquireDataLog(Path.GetFileNameWithoutExtension(functionFilename) + "_Amplitude" + amplitude.ToString("F1") + "_Offset" + selectedOffset.ToString("F1") + "_data.csv", dataLogHeader); runningStopwatch.Reset(); runningStopwatch.Start(); // Start task Hulk.SetCommandType(Hulk.CommandType.Velocity); Hulk.StartTask(); }
/// <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(); } } }