/// <summary>
/// Changes the angle of the pendulums and calculates the corrections for the feedback controllers.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
public void ChangeAngle(object sender, ElapsedEventArgs e)
{
if (DateTime.Now.Subtract(dateTime).TotalSeconds > WaitTimeForCalculation)
{
if (!initializeFeedbackControllers)
{
pid = new PID(kp, ki, kd, maxOutput);
adrc = new ADRC_PD(r, c, b, hModifier, kp, kd, maxOutput);
this.BeginInvoke((Action)(() =>
{
label1.Text = "Correction State: True";
}));
initializeFeedbackControllers = true;
}
if (useADRC)
{
currentOutput = adrc.Calculate(SetPoint, currentAngle);
}
else
{
currentOutput = pid.Calculate(SetPoint, currentAngle);
}
fileWriter.WriteLine(counter + "," + currentOutput + "," + currentAngle);
counter++;
PreviousAngles.Add((float)currentAngle);
PreviousOutputs.Add((float)currentOutput);
if (counter > FourierTransform.FourierMemory)
{
PreviousAngles.RemoveAt(0);
PreviousOutputs.RemoveAt(0);
}
}
Random rand = new Random();
double noise = rand.NextDouble() * NoiseFactor * (rand.Next(0, 1) * 2 - 1);
double invertedPendulumAngle = 0;
invertedPendulumAngle = invertedPendulum.Calculate(-currentOutput);
currentAngle = (invertedPendulumAngle + noise);
}
/// <summary>
/// Changes the angle of the pendulums and calculates the corrections for the feedback controllers.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
public void ChangeAngle(object sender, ElapsedEventArgs e)
{
if (DateTime.Now.Subtract(dateTime).TotalSeconds > WaitTimeForCalculation)
{
if (!initializeFeedbackControllers)
{
pid = new PID(kp, ki, kd, maxOutput);
adrc = new ADRC_PD(r, c, b, hModifier, kp, kd, maxOutput);
initializeFeedbackControllers = true;
}
correctionState = true;
outputPID = pid.Calculate(SetPoint, anglePID);
outputADRC = adrc.Calculate(SetPoint, angleADRC);
pidFileWriter.WriteLine(pidCounter + "," + outputPID + "," + anglePID);
adrcFileWriter.WriteLine(adrcCounter + "," + outputADRC + "," + angleADRC);
pidCounter++;
adrcCounter++;
ADRCAngle.Add((float)angleADRC);
PIDAngle.Add((float)anglePID);
ADRCOutput.Add((float)outputADRC);
PIDOutput.Add((float)outputPID);
if (ADRCAngle.ToArray().Length > FourierTransform.FourierMemory)
{
ADRCAngle.RemoveAt(0);
PIDAngle.RemoveAt(0);
ADRCOutput.RemoveAt(0);
PIDOutput.RemoveAt(0);
}
}
Random rand = new Random();
double noise = rand.NextDouble() * NoiseFactor * (rand.Next(0, 1) * 2 - 1);
double tempAnglePID = (invertedPendulumPID.Step(-outputPID) * 180 / Math.PI + noise); // % 360;
double tempAngleADRC = (invertedPendulumADRC.Step(-outputADRC) * 180 / Math.PI + noise); // % 360;
anglePID = tempAnglePID;
angleADRC = tempAngleADRC;
}
