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