Initial commit

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

@@ -0,0 +1,131 @@
+/*
+ * 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/control/async/asyncController.hpp"
+#include "okapi/api/control/controllerOutput.hpp"
+#include "okapi/api/control/iterative/iterativeController.hpp"
+#include "okapi/api/util/abstractRate.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <memory>
+
+namespace okapi {
+template <typename Input, typename Output> class ControllerRunner {
+  public:
+  /**
+   * A utility class that runs a closed-loop controller.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param ilogger The logger this instance will log to.
+   */
+  explicit ControllerRunner(const TimeUtil &itimeUtil,
+                            const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger())
+    : logger(ilogger), rate(itimeUtil.getRate()) {
+  }
+
+  /**
+   * Runs the controller until it has settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @return the error when settled
+   */
+  virtual Output runUntilSettled(const Input itarget, AsyncController<Input, Output> &icontroller) {
+    LOG_INFO("ControllerRunner: runUntilSettled(AsyncController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    while (!icontroller.isSettled()) {
+      rate->delayUntil(10_ms);
+    }
+
+    LOG_INFO("ControllerRunner: runUntilSettled(AsyncController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  /**
+   * Runs the controller until it has settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @param ioutput the output to write to
+   * @return the error when settled
+   */
+  virtual Output runUntilSettled(const Input itarget,
+                                 IterativeController<Input, Output> &icontroller,
+                                 ControllerOutput<Output> &ioutput) {
+    LOG_INFO("ControllerRunner: runUntilSettled(IterativeController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    while (!icontroller.isSettled()) {
+      ioutput.controllerSet(icontroller.getOutput());
+      rate->delayUntil(10_ms);
+    }
+
+    LOG_INFO("ControllerRunner: runUntilSettled(IterativeController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  /**
+   * Runs the controller until it has reached its target, but not necessarily settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @return the error when settled
+   */
+  virtual Output runUntilAtTarget(const Input itarget,
+                                  AsyncController<Input, Output> &icontroller) {
+    LOG_INFO("ControllerRunner: runUntilAtTarget(AsyncController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    double error = icontroller.getError();
+    double lastError = error;
+    while (error != 0 && std::copysign(1.0, error) == std::copysign(1.0, lastError)) {
+      lastError = error;
+      rate->delayUntil(10_ms);
+      error = icontroller.getError();
+    }
+
+    LOG_INFO("ControllerRunner: runUntilAtTarget(AsyncController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  /**
+   * Runs the controller until it has reached its target, but not necessarily settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @param ioutput the output to write to
+   * @return the error when settled
+   */
+  virtual Output runUntilAtTarget(const Input itarget,
+                                  IterativeController<Input, Output> &icontroller,
+                                  ControllerOutput<Output> &ioutput) {
+    LOG_INFO("ControllerRunner: runUntilAtTarget(IterativeController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    double error = icontroller.getError();
+    double lastError = error;
+    while (error != 0 && std::copysign(1.0, error) == std::copysign(1.0, lastError)) {
+      ioutput.controllerSet(icontroller.getOutput());
+      lastError = error;
+      rate->delayUntil(10_ms);
+      error = icontroller.getError();
+    }
+
+    LOG_INFO("ControllerRunner: runUntilAtTarget(IterativeController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  std::unique_ptr<AbstractRate> rate;
+};
+} // namespace okapi