motor.hpp

Go to the documentation of this file.

#pragma once
 
#include <cmath>
#include <numbers>
#include <optional>
#include <stdexcept>
#include <utility>
 
namespace libstp::foundation
{
    inline constexpr double kDefaultEncoderTicksPerRev = 1440.0;

    struct PidGains
    {
        double kp{1.0}, ki{0.0}, kd{0.0};
    };

    struct Feedforward
    {
        double kS{0.0};
        double kV{0.0};
        double kA{0.0};
    };

    struct MotorCalibration
    {
        double ticks_to_rad{2.0 * std::numbers::pi / kDefaultEncoderTicksPerRev};
        double vel_lpf_alpha{0.5};
        double bemf_offset{0.0};
        std::optional<PidGains> pid{};
        double static_friction_pct{0.0};
 
        MotorCalibration() = default;
 
        MotorCalibration(double ticks_to_rad,
                         double vel_lpf_alpha,
                         std::optional<PidGains> pid = std::nullopt,
                         double bemf_offset = 0.0,
                         double static_friction_pct = 0.0)
            : ticks_to_rad(ticks_to_rad),
              vel_lpf_alpha(vel_lpf_alpha),
              bemf_offset(bemf_offset),
              pid(std::move(pid)),
              static_friction_pct(static_friction_pct)
        {
        }

        void validate() const
        {
            if (!(ticks_to_rad > 0.0))
                throw std::invalid_argument("MotorCalibration: ticks_to_rad must be > 0");
            if (!(vel_lpf_alpha >= 0.0 && vel_lpf_alpha <= 1.0))
                throw std::invalid_argument("MotorCalibration: vel_lpf_alpha must be in [0, 1]");
            if (!std::isfinite(bemf_offset))
                throw std::invalid_argument("MotorCalibration: bemf_offset must be finite");
            if (!(std::isfinite(static_friction_pct) && static_friction_pct >= 0.0))
                throw std::invalid_argument("MotorCalibration: static_friction_pct must be finite and >= 0");
            if (pid && (!std::isfinite(pid->kp) || !std::isfinite(pid->ki) || !std::isfinite(pid->kd)))
                throw std::invalid_argument("MotorCalibration: pid gains must be finite");
        }
    };

    class FeedforwardController
    {
    public:
        Feedforward config;
 
        explicit FeedforwardController(Feedforward ff = {}) : config(ff) {}
 
        [[nodiscard]] double calculate(double velocity, double acceleration = 0.0) const
        {
            double output = config.kV * velocity + config.kA * acceleration;
            if (velocity != 0.0)
            {
                output += config.kS * std::copysign(1.0, velocity);
            }
            return output;
        }
    };
}