mecanum.hpp

Go to the documentation of this file.

#pragma once
#include "kinematics/kinematics.hpp"
#include "drive/motor_adapter.hpp"
#include "foundation/config.hpp"
#include "hal/IMotor.hpp"
#include <array>
#include <vector>
 
namespace libstp::kinematics::mecanum
{
    class MecanumKinematics : public IKinematics
    {
    private:
        double m_wheelbase;
        double m_trackWidth;
        double m_wheelRadius;
        double m_maxWheelSpeed{0.0}; // 0 = no limiting
 
        // Per-axis velocity-command gain [vx, vy, wz]; folded into fwd_matrix in
        // getStmOdometryConfig() to compensate drivetrain efficiency (e.g.
        // mecanum roller slip). Calibrated by the autotune velocity phase.
        std::array<double, 3> m_velCmdGain{1.0, 1.0, 1.0};
 
        drive::MotorAdapter front_left_motor_;
        drive::MotorAdapter front_right_motor_;
        drive::MotorAdapter back_left_motor_;
        drive::MotorAdapter back_right_motor_;
 
    public:
        MecanumKinematics(hal::motor::IMotor* front_left_motor,
                         hal::motor::IMotor* front_right_motor,
                         hal::motor::IMotor* back_left_motor,
                         hal::motor::IMotor* back_right_motor,
                         double wheelbase,
                         double trackWidth,
                         double wheelRadius,
                         foundation::VelocityCommandGain velocityCommandGain = {});
        ~MecanumKinematics() override = default;

        [[nodiscard]] std::size_t wheelCount() const override;

        MotorCommands applyCommand(const foundation::ChassisVelocity& cmd, double dt) override;

        [[nodiscard]] foundation::ChassisVelocity estimateState() const override;

        void hardStop() override;

        [[nodiscard]] bool supportsLateralMotion() const override;

        void resetEncoders() override;
 
        [[nodiscard]] double getWheelRadius() const override { return m_wheelRadius; }

        void setMaxWheelSpeed(double max_wheel_speed) { m_maxWheelSpeed = max_wheel_speed; }
        [[nodiscard]] double getMaxWheelSpeed() const { return m_maxWheelSpeed; }

        [[nodiscard]] double maxYawRateForArcRadius(double radius_m,
                                                    bool /*lateral*/) const override
        {
            if (m_maxWheelSpeed <= 0.0)
                return std::numeric_limits<double>::infinity();
            const double lever = 0.5 * (m_wheelbase + m_trackWidth);
            const double max_wheel_mps = m_maxWheelSpeed * m_wheelRadius;
            return max_wheel_mps / (radius_m + lever);
        }

        [[nodiscard]] std::vector<hal::motor::IMotor*> getMotors() override;
 
        [[nodiscard]] StmOdometryConfig getStmOdometryConfig() override;

        void setVelocityCommandGains(double gx, double gy, double gw) override;
        [[nodiscard]] std::array<double, 3> getVelocityCommandGains() const override;

        void applyPowerCommand(const foundation::ChassisVelocity& direction,
                               int power_percent) override;
    };
}