/// <summary> /// Calculate system dynamics /// </summary> /// <param name="voltageController">Reference to voltage controller</param> /// <param name="state">Actual system state</param> /// <param name="userAngle">Pendulum angle generated by user</param> /// <param name="targetPosition">Trajectory target position</param> /// <param name="verticalWindForce">Vertical wind force value</param> /// <param name="horizontalWindForce">Horizontal wind formce value</param> /// <param name="voltage">Result regulation voltage</param> /// <returns>New system state</returns> private OneDimensionalSystemState ExecuteSystemCalculations(IVoltageController voltageController, OneDimensionalSystemState state, double userAngle, double targetPosition, double verticalWindForce, double horizontalWindForce, out double voltage) { voltageController.SetTime(systemState.Time); voltageController.SetUserAngle(userAngle); voltageController.SetControlError(userAngle - state.Angle, targetPosition - state.Position); voltage = voltageController.GetVoltage(); systemState.SolverParameters.Voltage = voltage; systemState.SolverParameters.VerticalWindForce = verticalWindForce; systemState.SolverParameters.HorizontalWindForce = horizontalWindForce; solver.UpdateSystemParameters(systemState.SolverParameters); var t = systemState.ToTimeArray(); var x = state.ToStateArray(); var resultState = solver.SolveODESystem(t, x); if (Math.Abs(resultState.Angle) > systemState.MaxAngle) { MessageBox.Show("Pendulum lost controllability"); dispatcherTimer.Stop(); return(null); } if (Math.Abs(resultState.Position) > SceneControl.simulationArea.Size / 2 - SceneControl.cart.PlatformSize / 2) { resultState.Position = Math.Sign(resultState.Position) * (SceneControl.simulationArea.Size / 2 - SceneControl.cart.PlatformSize / 2); } return(resultState); }
/// <summary> /// Initialization of physics controllers /// </summary> private void InitializeControllers() { xCoordVoltageController = new VoltageController(); yCoordVoltageController = new VoltageController(); windController = new WindController(); trajectoryController = new TrajectoryController(); gameController = new GameController(UpKeyboardButton, DownKeyboardButton, LeftKeyboardButton, RightKeyboardButton); controllers = new List <IController>(); controllers.Add(xCoordVoltageController); controllers.Add(yCoordVoltageController); controllers.Add(windController); controllers.Add(trajectoryController); controllers.Add(gameController); }