pid.h

浏览该文件的文档.

 
#pragma once
#include <cmath>
#include <algorithm>
 
#include "device/motor/base.hpp"
#include "controller.hpp"
#include "utils/controller.hpp"
 
namespace roboctrl::utils{
template<typename T, auto error_measurer>
struct pid_base {
    T kp = 0;
    T ki = 0;
    T kd = 0;
 
    T max_out{};
    T max_iout{};
 
    using input_type = T;
    using state_type = T;
 
    struct params_type{
        T kp = 0;
        T ki = 0;
        T kd = 0;
 
        T max_out{};
        T max_iout{};
    };
 
    pid_base() = default;
 
    pid_base(const params_type& params)
        : kp(params.kp), ki(params.ki), kd(params.kd),
          max_out(params.max_out), max_iout(params.max_iout) {}
 
    void set_target(T target) { target_ = target; }
 
    inline T target() const { return target_; }
 
    void update(T current, T dt) {
        T error_prev = last_error_;
        T error_curr = error_measurer(current, target_);
        last_error_ = error_curr;
 
        T pout = kp * error_curr;
 
        if (dt > T{0}) {
            integral_ += ki * error_curr * dt;
        }
        integral_ = std::clamp(integral_, -max_iout, max_iout);
 
        const T derivative = dt > T{0}
            ? kd * (error_curr - error_prev) / dt
            : T{0};
 
        T out = pout + integral_ + derivative;
        out = std::clamp(out, -max_out, max_out);
 
        output_ = out;
    }
 
    void update(T current) { update(current, T{1}); }
 
    void update(T target, T current, T dt) {
        set_target(target);
        update(current, dt);
    }
 
    void clean() {
        integral_ = 0;
        last_error_ = 0;
        output_ = 0;
        target_ = 0;
    }
 
    T state() const { return output_; }
 
private:
    T target_ = 0;       
    T integral_ = 0;     
    T last_error_ = 0;   
    T output_ = 0;       
};
 
namespace details{
 
constexpr auto linear_error = [](fp32 cur, fp32 target) {
    return target - cur;
};
 
constexpr auto rad_error = [](fp32 cur, fp32 target) {
    fp32 diff = target - cur;
    diff = fmod(diff + M_PI, 2*M_PI);
    if (diff < 0) diff += 2*M_PI;
    diff -= M_PI;
    return diff;
};
 
}
 
using linear_pid = pid_base<fp32, details::linear_error>;
using rad_pid    = pid_base<fp32, details::rad_error>;
 
static_assert(utils::controller<linear_pid>);
static_assert(utils::controller<rad_pid>);
 
template<device::motor motor_type>
using linear_pid_motor = device::controlled_motor<motor_type, linear_pid>;
 
template<device::motor motor_type>
using rad_pid_motor = device::controlled_motor<motor_type, rad_pid>;
 
}