field_mecanum_logic.hpp

Go to the documentation of this file.

#ifndef FANG_ROBOTICS_MCB_CONTROL_CHASSIS_HOLONOMIC_MECANUM_LOGIC_FIELD_MECANUM_LOGIC_HPP
#define FANG_ROBOTICS_MCB_CONTROL_CHASSIS_HOLONOMIC_MECANUM_LOGIC_FIELD_MECANUM_LOGIC_HPP
#include "control/chassis/data/physics_alias.hpp"
#include "wrap/units/units_alias.hpp"
#include "robot_mecanum_logic.hpp"
 
using namespace units::literals;
 
namespace fang::chassis
{
    class FieldMecanumLogic
    {
    public:
        using Config = RobotMecanumLogic::Config;
 
        FieldMecanumLogic(const Config& config);
 
        FieldMecanumLogic(const Meters& horizontalWheelDistance,
                          const Meters& verticalWheelDistance,
                          const Meters& wheelRadius);
        void setMotion(const Velocity2D& translation, const RPM& rotation);
        void setTotalMotion(const Velocity2D& translation, const RPM& rotation, const Radians& robotAngle);
        void setRobotAngle(const Radians& robotAngle);
        void setTranslation(const Velocity2D& translation);
        void setRotation(const RPM& rotation);
        const Radians&getRobotAngle() const;
        const Velocity2D& getTranslation() const;
        RPM getRotation() const;
        QuadRPM getWheelSpeeds() const;
    private:
        RPM getFrontRightWheelSpeed() const;
        RPM getFrontLeftWheelSpeed() const;
        RPM getRearLeftWheelSpeed() const;
        RPM getRearRightWheelSpeed() const;
        //This does not update the translation when the robot angle is changed.
        //It is useful in certain cases to speed stuff up.
        void rawSetRobotAngle(const Radians& rotation);
        Velocity2D fieldToRobotTranslation(const Velocity2D& translation) const;
        Velocity2D robotToFieldTranslation(const Velocity2D& translation) const;
 
        Radians robotAngle_{0.0};
        RobotMecanumLogic robotMecanumLogic_;
        Velocity2D fieldTranslation_{0_mps, 0_mps};
    };
}
#endif