/// <summary> /// Create a handler for the given trial parameters file. /// </summary> /// <param name="filename">Filename of the CSV file containing the trial parameters</param> internal static bool Read(String filename) { CsvReader csvReader = new CsvReader(); csvReader.readFile(filename); bool readSuccessful = true; list.Clear(); for (int i = 0; i < csvReader.NumLinesRead; i++) { Trial t = new Trial(csvReader, i); if (t.SetNoiseProfile()) { list.Add(t); } else { readSuccessful = false; } } if (readSuccessful) { String filenameWithoutDirectory; filenameWithoutDirectory = filename.Substring(filename.LastIndexOf('\\')); protocolFilename = filenameWithoutDirectory.Substring(1, filenameWithoutDirectory.Length - 5); } else { list.Clear(); } return(readSuccessful); }
/// <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> /// 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); }