bemf_velocity_tune.hpp

Go to the documentation of this file.

#pragma once
 
#include <array>
#include <string>
#include <vector>
 
#include "autotune/types.hpp"
#include "drive/drive.hpp"
#include "hal/odometry.hpp"
 
namespace libstp::autotune
{
    struct BemfVelocityConfig
    {
        std::vector<int> pwm_levels{};
        int    pwm_min_percent{30};
        int    pwm_max_percent{90};
        int    pwm_steps{6};
        int    sweeps{1};

        double pre_roll_distance_m{0.10};
        double measure_distance_m{0.15};
        double segment_timeout_s{5.0};
        int    return_pwm_percent{35};
        double return_tolerance_m{0.04};
        double return_timeout_s{6.0};
 
        int    sample_hz{100};
        double settle_s{0.5};

        double min_window_distance_m{0.03};
        double min_window_ticks{20.0};

        bool   require_calib_board{true};
        bool   apply{true};
    };

    struct BemfVelocityPoint
    {
        int    pwm_percent{0};
        double ground_truth_distance_m{0.0}; 
        double window_time_s{0.0};
        double body_speed_mps{0.0};          
        double wheel_omega_rad_s{0.0};       
        std::array<double, 4> delta_ticks{}; 
        std::array<double, 4> median_bemf{}; 
        std::array<double, 4> ticks_to_rad{};
        bool   valid{false};
    };

    struct BemfMotorFit
    {
        int    port{-1};
        int    n_points{0};
        double ticks_to_rad_median{0.0}; 
        double ticks_to_rad_mean{0.0};
        double ticks_to_rad_cv{0.0};     
        // ω = slope·bemf + intercept  (ω from ground truth, bemf from ADC).
        double bemf_omega_slope{0.0};
        double bemf_omega_intercept{0.0};
        double bemf_omega_r2{0.0};
        double bemf_offset{0.0};
        bool   linear{false};            
    };

    struct BemfVelocityResult
    {
        std::vector<BemfVelocityPoint> points{};
        std::array<BemfMotorFit, 4>    motors{};
        std::array<double, 4>          ticks_to_rad{}; 
        std::array<double, 4>          bemf_offset{};  
        bool   linear_overall{false};
        bool   applied{false};
        bool   success{false};
        std::string failure_reason{};
    };

    class BemfVelocityTuner
    {
    public:
        BemfVelocityTuner(drive::Drive& drive, odometry::IOdometry& odometry);
 
        [[nodiscard]] BemfVelocityResult tune(const BemfVelocityConfig& cfg) const;
 
    private:
        drive::Drive&        drive_;
        odometry::IOdometry& odometry_;
    };
 
} // namespace libstp::autotune