// Manually slide the Fulcrum position Fulcrum SlideFulcrum() { Fulcrum fulcrum = new Fulcrum(); fulcrum.x = FulcrumSlider.Value / 10; fulcrum.v = (fulcrum.x - CurrentFulcrum.x) / dt; fulcrum.a = (fulcrum.v - CurrentFulcrum.v) / dt; return(fulcrum); }
// Reset all parameters and Initialize the Pendulum void ResetPendulum() { CurrentPendulum = new Pendulum(); CurrentFulcrum = new Fulcrum(); CurrentPendulum.theta = theta0 / 180 * Math.PI; FulcrumSlider.Value = 0; Idtheta = 0; Idx = 0; slowdowned = false; n = 1; DisplayPendulum(0, CurrentPendulum.theta); }
// Oscillating Fulcrum for Swing up the Pendulum Fulcrum OscillateFulcrum(double t) { double A = 1.5; // Amplitude double T = Math.Sqrt(g / CurrentPendulum.L) * 0.7; // Period double omega = 2 * Math.PI / T; // Angular frequency Fulcrum fulcrum = new Fulcrum() { x = A * Math.Sin(omega * t) + X0, v = A * omega * Math.Cos(omega * t), a = -A *Math.Pow(omega, 2) * Math.Sin(omega * t) }; return(fulcrum); }
// When fail balancing, return the Pendulum to the center for next try Fulcrum ReturnPendulum() { Fulcrum fulcrum = new Fulcrum(); if (CurrentFulcrum.x > 3.0) { fulcrum.v = -10.0; } if (CurrentFulcrum.x < -3.0) { fulcrum.v = 10.0; } fulcrum.x = CurrentFulcrum.x + fulcrum.v * 0.1; fulcrum.a = 0.0; return(fulcrum); }
// Balancing the Pendulum by PID-controlling the Fulcrum position Fulcrum BalancePendulum(double dtheta) { Fulcrum fulcrum = new Fulcrum(); Idtheta += (dtheta * dt); double v = -CurrentPendulum.omega; // Balancing by PID-Control double dx = 0.6 * Ku1 * (dtheta + Idtheta / (0.5 * Pu1) + 0.125 * Pu1 * v); //double dx = Kp * dtheta - Kd * CurrentPendulum.omega + Ki * Idtheta; // Actuate the Fulcrum position fulcrum.x = CurrentFulcrum.x + dx; fulcrum.v = (fulcrum.x - CurrentFulcrum.x) / dt; fulcrum.a = (fulcrum.v - CurrentFulcrum.v) / dt; return(fulcrum); }
// Control the Pendulum void ControlPendulum() { double t = 0; double dtheta = 0; Device.StartTimer(TimeSpan.FromMilliseconds(100), () => { bool NextTimerStart = true; switch (control) { case "StandBy": CurrentFulcrum = SlideFulcrum(); X0 = CurrentFulcrum.x; break; case "Stop": X0 = CurrentFulcrum.x; CurrentFulcrum.v = 0; CurrentFulcrum.a = 0; FulcrumSlider.Value = X0 * 10.0; t = 0; control = "StandBy"; break; case "Pause": return(NextTimerStart); case "Reset": // Initialize all values ResetPendulum(); control = "StandBy"; break; case "SwingUp": // Oscille and SwingUp Fulcrum position CurrentFulcrum = OscillateFulcrum(t); t += dt; // Increment time if (Math.Abs(CurrentPendulum.theta) > 3.0) { control = "Balance"; } break; case "Balance": // Balance Control Pendulum with PID-Control dtheta = SlowDownPendulum(); CurrentFulcrum = BalancePendulum(dtheta); if (slowdowned) { control = "Move"; } if (Math.Abs(CurrentPendulum.theta) < 1.5) { control = "Retry"; } break; case "Move": // Horizontally move the Pendulum dtheta = MovePendulum(FulcrumSlider.Value / 10); CurrentFulcrum = BalancePendulum(dtheta); if (Math.Abs(CurrentPendulum.theta) < 1.5) // When fail balancing { Idtheta = 0; Idx = 0; CurrentPendulum.k = 30.0; // Slowdown the Pendulum rotation control = "Retry"; } break; case "Retry": CurrentFulcrum = ReturnPendulum(); if (Math.Abs(CurrentPendulum.omega) < 2.0) { CurrentPendulum.k = 0.5; // Return the Damping Coefficient Idtheta = 0; Idx = 0; slowdowned = false; n = 1; t = 0; X0 = CurrentFulcrum.x; control = "SwingUp"; } break; } StatusLabel.Text = "Status: " + control; // Calculate the Pendulram altitude CurrentPendulum = RungeKutta(t, CurrentPendulum.theta, CurrentPendulum.omega); // Display the Pendulum DisplayPendulum(CurrentFulcrum.x / 10, CurrentPendulum.theta); return(NextTimerStart); }); }