controller.hpp

浏览该文件的文档.

 
#pragma once
 
#include <utility>
#include <functional>
#include <memory>
#include <vector>
 
#include "utils/utils.hpp"
 
namespace roboctrl::utils{
 
template<typename T>
class control_stage {
public:
    virtual ~control_stage() = default;
    virtual T update(T input, fp32 dt) = 0;
    virtual void reset() = 0;
};
 
template<typename T>
class runtime_control_chain {
public:
    void add(std::unique_ptr<control_stage<T>> stage) {
        if (stage) stages_.push_back(std::move(stage));
    }
 
    T update(T input, fp32 dt) {
        for (auto& stage : stages_) input = stage->update(input, dt);
        return input;
    }
 
    void reset() {
        for (auto& stage : stages_) stage->reset();
    }
 
    [[nodiscard]] bool empty() const noexcept { return stages_.empty(); }
 
private:
    std::vector<std::unique_ptr<control_stage<T>>> stages_;
};
 
template<typename T>
concept controller = requires (T c) {
    typename T::state_type;
    typename T::input_type;
    typename T::params_type;
    { T(std::declval<typename T::params_type>()) };
    { c.update(std::declval<typename T::input_type>()) } -> std::same_as<void>;
    { c.state() } -> std::same_as<typename T::state_type>;
};
 
template<controller... Cs>
struct control_chain {
    static_assert(sizeof...(Cs) > 0);
 
    template<std::size_t N>
    using state_type_n = typename std::tuple_element<N, std::tuple<typename Cs::state_type...>>::type;
 
    template<std::size_t N>
    using input_type_n = typename std::tuple_element<N, std::tuple<typename Cs::input_type...>>::type;
 
    using state_type = state_type_n<sizeof...(Cs) - 1>;
    using input_type = input_type_n<0>;
    using params_type = std::tuple<typename Cs::params_type...>;
 
    std::tuple<Cs...> controllers_;
 
    explicit control_chain(Cs&&... cs)
        : controllers_(std::forward<Cs>(cs)...) {}
 
    explicit control_chain(params_type&& params)
        : controllers_(std::apply(
            [](auto&&... ps){ return std::make_tuple(Cs(std::forward<decltype(ps)>(ps))...); },
            std::move(params)))
    {}
 
 
    void update(const input_type& input) {
        auto current_input = input;
        std::apply([&](auto&... ctrls){
            ((ctrls.update(current_input),
              current_input = ctrls.state()), ...);
        }, controllers_);
    }
 
    state_type state() const {
        return std::get<sizeof...(Cs) - 1>(controllers_).state();
    }
};
 
template<controller controller_type>
controller_type make_controller(const typename controller_type::info_type& info){
    return {info};
}
 
}