/// <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()); }