differential.hpp

Go to the documentation of this file.

#pragma once
#include "kinematics/kinematics.hpp"
#include "drive/motor_adapter.hpp"
#include "hal/IMotor.hpp"
#include <vector>
 
namespace libstp::kinematics::differential
{
    class DifferentialKinematics : public IKinematics
    {
    private:
        double m_wheelbase;
        double m_wheelRadius;
        double m_maxWheelSpeed{0.0}; // 0 = no limiting
 
        drive::MotorAdapter left_motor_;
        drive::MotorAdapter right_motor_;
 
    public:
        DifferentialKinematics(hal::motor::IMotor* left_motor,
                               hal::motor::IMotor* right_motor,
                               double wheelbase,
                               double wheelRadius);
        ~DifferentialKinematics() 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; }

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

        void applyPowerCommand(const foundation::ChassisVelocity& direction,
                               int power_percent) override;
 
        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 max_wheel_mps = m_maxWheelSpeed * m_wheelRadius;
            return max_wheel_mps / (radius_m + 0.5 * m_wheelbase);
        }
    };
}