SimWorld.hpp

Go to the documentation of this file.

#pragma once
 
#include "libstp/sim/Pose2D.hpp"
#include "libstp/sim/RobotConfig.hpp"
#include "libstp/sim/WorldMap.hpp"
 
#include <array>
#include <cstdint>
#include <optional>
#include <random>
#include <string>
#include <unordered_map>
#include <vector>
 
namespace libstp::sim
{
    enum class DrivetrainKind : uint8_t
    {
        Differential,   
        Mecanum,        
    };

    struct SimMotorMap
    {
        DrivetrainKind kind{DrivetrainKind::Differential};
 
        // Differential drive: left + right wheel ports.
        uint8_t leftPort{0};
        uint8_t rightPort{1};
        bool leftInverted{false};
        bool rightInverted{false};
 
        // Mecanum drive: front-left, front-right, back-left, back-right ports.
        uint8_t flPort{0};
        uint8_t frPort{1};
        uint8_t blPort{2};
        uint8_t brPort{3};
        bool flInverted{false};
        bool frInverted{false};
        bool blInverted{false};
        bool brInverted{false};

        float maxWheelVelocityRadS{30.0f};

        float motorTimeConstantSec{0.1f};

        float ticksToRad{0.00436332313f};

        float bemfSampleRateHz{200.0f};

        float viscousDragCoeff{0.0f};

        float coulombFrictionRadSS{0.0f};

        float bemfNoiseStddev{0.0f};
    };

    class SimWorld
    {
    public:
        SimWorld() = default;
 
        void configure(const RobotConfig& robot, const SimMotorMap& motors);
        void setMap(WorldMap map);
        const WorldMap& map() const noexcept { return m_map; }

        int currentLayer() const noexcept { return m_currentLayer; }
        void setCurrentLayer(int idx) noexcept { m_currentLayer = idx; }

        void attachLineSensor(uint8_t analogPort, float forwardCm, float strafeCm,
                              std::string name = {});

        void attachDistanceSensor(uint8_t analogPort, float forwardCm, float strafeCm,
                                  float mountAngleRad = 0.0f,
                                  float maxRangeCm = 100.0f,
                                  std::string name = {});

        void detachSensor(uint8_t analogPort);

        std::optional<uint16_t> readAnalog(uint8_t analogPort) const;
 
        void setPose(const Pose2D& pose);
        Pose2D pose() const noexcept { return m_pose; }
        float yawRateRadS() const noexcept { return m_yawRateRadS; }

        Pose2D odometryPose() const noexcept { return m_odoPose; }

        struct OdometryDrift
        {
            float vxBias{1.0f};
            float vyBias{1.0f};
            float yawBias{1.0f};
        };
        void setOdometryDrift(float vxBias, float vyBias, float yawBias) noexcept
        {
            m_odoDrift = OdometryDrift{vxBias, vyBias, yawBias};
        }
        OdometryDrift odometryDrift() const noexcept { return m_odoDrift; }

        void setMotorCommand(uint8_t port, int signedPercent);

        void setMotorVelocityCommand(uint8_t port, int bemfUnits);

        int motorBemf(uint8_t port) const;

        int motorPositionTicks(uint8_t port) const;

        void tick(float dt);

        Vec2 sensorWorldPosition(const SensorMount& mount) const;

        bool isSensorOnBlackLine(const SensorMount& mount) const;

        float wheelOmegaLeft() const noexcept { return m_wheelOmega[kLeft]; }
        float wheelOmegaRight() const noexcept { return m_wheelOmega[kRight]; }

        float wheelOmega(std::size_t idx) const noexcept
        {
            return idx < m_wheelOmega.size() ? m_wheelOmega[idx] : 0.0f;
        }
 
        const std::vector<Pose2D>& trace() const noexcept { return m_trace; }
        void clearTrace() { m_trace.clear(); }
 
    private:
        static constexpr std::size_t kNumMotorPorts = 4;
        static constexpr std::size_t kMaxWheels = 4;
        static constexpr std::size_t kLeft = 0;
        static constexpr std::size_t kRight = 1;
        // Mecanum-specific aliases for clarity.
        static constexpr std::size_t kFL = 0;
        static constexpr std::size_t kFR = 1;
        static constexpr std::size_t kBL = 2;
        static constexpr std::size_t kBR = 3;
 
        enum class SensorKind : uint8_t { Line, Distance };
 
        struct SensorEntry
        {
            SensorKind kind{SensorKind::Line};
            SensorMount mount{};
            float mountAngleRad{0.0f};  // distance sensor aim direction in robot frame
            float maxRangeCm{100.0f};
        };
 
        RobotConfig m_robot{};
        SimMotorMap m_motors{};
        WorldMap m_map{};
        int m_currentLayer{0};  // index of the plane the robot is on (ground = 0)
        Pose2D m_pose{};
        Pose2D m_odoPose{};  // dead-reckoned controller-facing odometry (see odometryPose())
        OdometryDrift m_odoDrift{};  // sim-only per-axis odometry bias (see setOdometryDrift)
 
        std::array<float, kNumMotorPorts> m_motorTargetOmega{};   // rad/s, signed
        std::array<float, kMaxWheels> m_wheelOmega{};             // rad/s
        std::array<float, kMaxWheels> m_wheelAngleRad{};          // rad, signed
        float m_yawRateRadS{0.0f};
        std::vector<Pose2D> m_trace;
        std::unordered_map<uint8_t, SensorEntry> m_sensors;

        void applyLayerTransitions(const Pose2D& prev, const Pose2D& cur);
 
        float targetOmega(std::size_t wheelIndex) const;
        uint16_t sampleLineSensor(const SensorEntry&) const;
        uint16_t sampleDistanceSensor(const SensorEntry&) const;
        std::size_t portToWheelIndex(uint8_t port) const;
        bool wheelInverted(std::size_t wheelIndex) const;
        uint8_t wheelPort(std::size_t wheelIndex) const;
        std::size_t activeWheelCount() const noexcept;

        mutable std::mt19937 m_rng{42};
        mutable std::normal_distribution<float> m_noiseDist{0.0f, 1.0f};
    };
}