field_drift_command.hpp

Go to the documentation of this file.

#pragma once
#include "control/chassis/drive/holonomic/holonomic_subsystem.hpp"
#include "wrap/rail/chassis/iholonomic_input.hpp"
 
#include "tap/control/command.hpp"
 
namespace fang::chassis
{
    class FieldDriftCommand : public tap::control::Command
    {
    public:
        struct Config
        {
            physics::Velocity2D maxTranslation;
            RPM maxRotation;
        };
 
        FieldDriftCommand
        (
            HolonomicSubsystem& holonomicSubsystem,
            IHolonomicInput& holonomicInput,
            const Config& config
        );
 
        virtual ~FieldDriftCommand() = default;
 
        const char* getName() const override;
        void initialize() override;
        void execute() override;
        void end(bool interrupted) override;
        bool isFinished() const;
 
    protected:
        physics::Velocity2D getFieldTranslation() const;
 
        RPM getFieldRotation() const;
 
        static constexpr const char* kName{"Field Drift"};
 
        HolonomicSubsystem& holonomicSubsystem_;
        IHolonomicInput& holonomicInput_;
        const Config kConfig_;
 
    private:
        static void assertConfigValues(const Config& config);
        static void assertGetFieldTranslationUniformSigns
        (
            const math::AbstractVector2D& abstractFieldTranslation,
            const physics::Velocity2D& fieldTranslation
        );
    };
}//namespace chassis