controllerRunner.hpp

Go to the documentation of this file.

/*
 * 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:
  explicit ControllerRunner(const TimeUtil &itimeUtil,
                            const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger())
    : logger(ilogger), rate(itimeUtil.getRate()) {
  }
 
  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();
  }
 
  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();
  }
 
  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();
  }
 
  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