Initial commit

include/okapi/api/control/util/settledUtil.hpp

@@ -0,0 +1,51 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/units/QTime.hpp"
+#include "okapi/api/util/abstractTimer.hpp"
+#include <memory>
+
+namespace okapi {
+class SettledUtil {
+  public:
+  /**
+   * A utility class to determine if a control loop has settled based on error. A control loop is
+   * settled if the error is within `iatTargetError` and `iatTargetDerivative` for `iatTargetTime`.
+   *
+   * @param iatTargetTimer A timer used to track `iatTargetTime`.
+   * @param iatTargetError The minimum error to be considered settled.
+   * @param iatTargetDerivative The minimum error derivative to be considered settled.
+   * @param iatTargetTime The minimum time within atTargetError to be considered settled.
+   */
+  explicit SettledUtil(std::unique_ptr<AbstractTimer> iatTargetTimer,
+                       double iatTargetError = 50,
+                       double iatTargetDerivative = 5,
+                       QTime iatTargetTime = 250_ms);
+
+  virtual ~SettledUtil();
+
+  /**
+   * Returns whether the controller is settled.
+   *
+   * @param ierror The current error.
+   * @return Whether the controller is settled.
+   */
+  virtual bool isSettled(double ierror);
+
+  /**
+   * Resets the "at target" timer and clears the previous error.
+   */
+  virtual void reset();
+
+  protected:
+  double atTargetError = 50;
+  double atTargetDerivative = 5;
+  QTime atTargetTime = 250_ms;
+  std::unique_ptr<AbstractTimer> atTargetTimer;
+  double lastError = 0;
+};
+} // namespace okapi