Motor.hpp

Go to the documentation of this file.

#pragma once
 
#include <chrono>
 
#include "foundation/motor.hpp"
#include "hal/IMotor.hpp"
#include "hal/PortRegistry.hpp"
 
namespace libstp::hal::motor
{
    class Motor : public IMotor
    {
#ifdef SAFETY_CHECKS_ENABLED
        static inline detail::PortRegistry registry_{};
 
        static void registerMotorPort(int port);
 
        static void unregisterMotorPort(int port);
#endif
 
    public:
        explicit Motor(int port, bool inverted, const foundation::MotorCalibration& calibration = {});
 
        ~Motor() override;

        void setSpeed(int percent) override;
        void setVelocity(int velocity) override;
        void moveToPosition(int velocity, int goalPosition) override;
        void moveRelative(int velocity, int deltaPosition) override;
        [[nodiscard]] int getPosition() const override;
        [[nodiscard]] int getBemf() const override;
        [[nodiscard]] bool isDone() const override;

        void brake() override;
        void off() override;
        void resetPositionCounter() override;
 
        [[nodiscard]] const foundation::MotorCalibration& getCalibration() const override;
        void setCalibration(const foundation::MotorCalibration& calibration) override;
        [[nodiscard]] int getPort() const override { return port_; }
        [[nodiscard]] bool isInverted() const override { return inverted_; }

        void setFirmwarePidGains(float kp, float ki, float kd) override;

        static void disableAll();
        static void enableAll();
 
        // --------------------------------------------------------------------
        // Test/simulation hooks (active on mock platform; no-op on wombat).
        //
        // The members are declared in the shared class so test code that holds
        // a `Motor*` (and the autotune tests) can interrogate / control the
        // simulated motor without an upcast. On the wombat platform these are
        // never written and `getBemf()` ignores them.
        // --------------------------------------------------------------------

        void setSimulatedBemfResponse(int steady_state_bemf, double time_constant_s);

        void clearSimulatedBemfResponse();

        void getLastFirmwarePidGains(float& kp, float& ki, float& kd) const override;
 
    private:
        int port_;
        bool inverted_;
        foundation::MotorCalibration calibration_;
 
        // --- Simulation / test state (mock platform writes; wombat ignores) --
        bool   sim_active_{false};
        int    sim_steady_state_{0};
        double sim_time_constant_s_{0.1};
        std::chrono::steady_clock::time_point sim_t_ref_{};
 
        float  last_fw_kp_{0.0f};
        float  last_fw_ki_{0.0f};
        float  last_fw_kd_{0.0f};
    };
}