132 lines
4.6 KiB
C++
132 lines
4.6 KiB
C++
/*
|
|
* 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
|