asyncWrapper.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/controllerInput.hpp"
#include "okapi/api/control/iterative/iterativeController.hpp"
#include "okapi/api/control/util/settledUtil.hpp"
#include "okapi/api/coreProsAPI.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/mathUtil.hpp"
#include "okapi/api/util/supplier.hpp"
#include <atomic>
#include <memory>
 
namespace okapi {
template <typename Input, typename Output>
class AsyncWrapper : virtual public AsyncController<Input, Output> {
  public:
  AsyncWrapper(const std::shared_ptr<ControllerInput<Input>> &iinput,
               const std::shared_ptr<ControllerOutput<Output>> &ioutput,
               const std::shared_ptr<IterativeController<Input, Output>> &icontroller,
               const Supplier<std::unique_ptr<AbstractRate>> &irateSupplier,
               const double iratio = 1,
               std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger())
    : logger(std::move(ilogger)),
      rateSupplier(irateSupplier),
      input(iinput),
      output(ioutput),
      controller(icontroller),
      ratio(iratio) {
  }
 
  AsyncWrapper(AsyncWrapper<Input, Output> &&other) = delete;
 
  AsyncWrapper<Input, Output> &operator=(AsyncWrapper<Input, Output> &&other) = delete;
 
  ~AsyncWrapper() override {
    dtorCalled.store(true, std::memory_order_release);
    delete task;
  }
 
  void setTarget(const Input itarget) override {
    LOG_INFO("AsyncWrapper: Set target to " + std::to_string(itarget));
    hasFirstTarget = true;
    controller->setTarget(itarget * ratio);
    lastTarget = itarget;
  }
 
  void controllerSet(const Input ivalue) override {
    controller->controllerSet(ivalue);
  }
 
  Input getTarget() override {
    return controller->getTarget();
  }
 
  Input getProcessValue() const override {
    return controller->getProcessValue();
  }
 
  Output getOutput() const {
    return controller->getOutput();
  }
 
  Output getError() const override {
    return controller->getError();
  }
 
  bool isSettled() override {
    return isDisabled() || controller->isSettled();
  }
 
  void setSampleTime(const QTime &isampleTime) {
    controller->setSampleTime(isampleTime);
  }
 
  void setOutputLimits(const Output imax, const Output imin) {
    controller->setOutputLimits(imax, imin);
  }
 
  void setControllerSetTargetLimits(double itargetMax, double itargetMin) {
    controller->setControllerSetTargetLimits(itargetMax, itargetMin);
  }
 
  Output getMaxOutput() {
    return controller->getMaxOutput();
  }
 
  Output getMinOutput() {
    return controller->getMinOutput();
  }
 
  void reset() override {
    LOG_INFO_S("AsyncWrapper: Reset");
    controller->reset();
    hasFirstTarget = false;
  }
 
  void flipDisable() override {
    LOG_INFO("AsyncWrapper: flipDisable " + std::to_string(!controller->isDisabled()));
    controller->flipDisable();
    resumeMovement();
  }
 
  void flipDisable(const bool iisDisabled) override {
    LOG_INFO("AsyncWrapper: flipDisable " + std::to_string(iisDisabled));
    controller->flipDisable(iisDisabled);
    resumeMovement();
  }
 
  bool isDisabled() const override {
    return controller->isDisabled();
  }
 
  void waitUntilSettled() override {
    LOG_INFO_S("AsyncWrapper: Waiting to settle");
 
    auto rate = rateSupplier.get();
    while (!isSettled()) {
      rate->delayUntil(motorUpdateRate);
    }
 
    LOG_INFO_S("AsyncWrapper: Done waiting to settle");
  }
 
  void startThread() {
    if (!task) {
      task = new CrossplatformThread(trampoline, this, "AsyncWrapper");
    }
  }
 
  CrossplatformThread *getThread() const {
    return task;
  }
 
  protected:
  std::shared_ptr<Logger> logger;
  Supplier<std::unique_ptr<AbstractRate>> rateSupplier;
  std::shared_ptr<ControllerInput<Input>> input;
  std::shared_ptr<ControllerOutput<Output>> output;
  std::shared_ptr<IterativeController<Input, Output>> controller;
  bool hasFirstTarget{false};
  Input lastTarget;
  double ratio;
  std::atomic_bool dtorCalled{false};
  CrossplatformThread *task{nullptr};
 
  static void trampoline(void *context) {
    if (context) {
      static_cast<AsyncWrapper *>(context)->loop();
    }
  }
 
  void loop() {
    auto rate = rateSupplier.get();
    while (!dtorCalled.load(std::memory_order_acquire) && !task->notifyTake(0)) {
      if (!isDisabled()) {
        output->controllerSet(controller->step(input->controllerGet()));
      }
 
      rate->delayUntil(controller->getSampleTime());
    }
  }
 
  virtual void resumeMovement() {
    if (isDisabled()) {
      // This will grab the output *when disabled*
      output->controllerSet(controller->getOutput());
    } else {
      if (hasFirstTarget) {
        setTarget(lastTarget);
      }
    }
  }
};
} // namespace okapi