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