mecanum_drive.hpp

Go to the documentation of this file.

#pragma once
 
#include "driver/drivers.hpp"
#include "driver/motor/data/directionality.hpp"
#include "control/chassis/drive/holonomic/mecanum/logic/field_mecanum_logic.hpp"
#include "control/chassis/drive/holonomic/holonomic_subsystem.hpp"
#include "util/physics/data/velocity_2d.hpp"
 
#include "wrap/units/units_alias.hpp"
#include "wrap/trap/communication/sensors/imu.hpp"
#include "wrap/rail/chassis/iquad_drive.hpp"
 
namespace fang::chassis
{
    class MecanumDrive : public HolonomicSubsystem
    {
    public:
        using Imu = trap::communication::sensors::IImu;
 
    struct ChassisDimensionConfig
    {
        Meters wheelRadius;
        Meters verticalWheelDistance;
        Meters horizontalWheelDistance;
    };
 
    struct Config
    {
        FieldMecanumLogic::Config mecanumLogicConfig;
    };
 
    MecanumDrive
    (
        Drivers& drivers,
        std::unique_ptr<IQuadDrive> quadDrive,
        std::unique_ptr<Imu> imu,
        const Config& config
    );
 
    void setTargetTranslation(const physics::Velocity2D& translation) override;
 
    void setTargetRotation(const RPM& rotation) override;
 
    void initialize() override;
    void update() override;
 
    void refresh() override;
 
    void refreshSafeDisconnect() override;
    private:
        void updateSystems();
        void updateFieldAngle();
        void syncWheelsToLogic();
 
        std::unique_ptr<IQuadDrive> quadDrive_;
        std::unique_ptr<Imu> imu_;
 
        FieldMecanumLogic mecanumLogic_;
    };
}//namespace fang::chassis