robot_mecanum_logic.hpp

Go to the documentation of this file.

#ifndef FANG_ROBOTICS_MCB_CONTROL_CHASSIS_HOLONOMIC_MECANUM_LOGIC_ROBOT_MECANUM_LOGIC_HPP
#define FANG_ROBOTICS_MCB_CONTROL_CHASSIS_HOLONOMIC_MECANUM_LOGIC_ROBOT_MECANUM_LOGIC_HPP
#include "abstract_field_mecanum_logic.hpp"
#include "control/chassis/data/physics_alias.hpp"
#include "wrap/units/units_alias.hpp"
 
namespace fang::chassis
{
    class RobotMecanumLogic
    {
    public:
 
        struct Config
        {
            const Meters& horizontalWheelDistance;
            const Meters& verticalWheelDistance;
            const Meters& wheelRadius;
        };
        RobotMecanumLogic(const Config& config);
 
        RobotMecanumLogic(const Meters& horizontalWheelDistance,
                          const Meters& verticalWheelDistance,
                          const Meters& wheelRadius);
        void setMotion(const Velocity2D& translation, const RPM& rotation);
        void setTranslation(const Velocity2D& translation);
        void setRotation(const RPM& rotation);
        const Velocity2D& getTranslation() const;
        RPM getRotation() const;
 
        QuadRPM getWheelSpeeds() const;
 
    private:
        RPM getFrontRightWheelSpeed() const;
        RPM getFrontLeftWheelSpeed() const;
        RPM getRearLeftWheelSpeed() const;
        RPM getRearRightWheelSpeed() const;
 
        Velocity2D translation_{MetersPerSecond{0.0}, MetersPerSecond{0.0}};
        RadiansPerSecond rotation_{0};
        const MetersPerRadians kWheelRadius_; //Required for the math to work, "how many meters for each radius?"
        const Meters kHorizontalWheelDistance_;
 
        //https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf
        //These are the constants used within the euqation
        const Meters kHalfHorizontalWheelDistance_{kHorizontalWheelDistance_ / 2.0};
        const Meters kVerticalWheelDistance_;
        const Meters kHalfVerticalWheelDistance_{kVerticalWheelDistance_ / 2.0};
        //l_x + l_y
        const MetersPerRadians kWheelDistanceConstant_{(kHalfVerticalWheelDistance_ + kHalfHorizontalWheelDistance_) / Radians{1}}; //The chassis is a larger "wheel"
    };
}
#endif