types.hpp

Go to the documentation of this file.

#pragma once
 
#include <array>
#include <cstdint>
#include <string>
#include <vector>
 
#include "foundation/motor.hpp"  // PidGains, Feedforward
 
namespace libstp::autotune
{
    struct AxisResult
    {
        double max_velocity{0.0};
        double acceleration{0.0};
        double deceleration{0.0};
    };

    struct CharacterizeConfig
    {
        int power_percent{100};
        int trials{3};
        double accel_timeout{3.0};
        double decel_timeout{3.0};
        int sample_hz{500};
    };
 
    // ========================================================================
    // Phase 2 — VelocityTuner types
    // ========================================================================

    struct StepResponseData
    {
        std::vector<double> times;      
        std::vector<double> commanded;  
        std::vector<double> measured;   
    };

    struct PlantParams
    {
        double Ks{0.0};    
        double Tu{0.0};    
        double Tg{0.0};    
        std::string method; 
    };

    struct VelocityTuneResult
    {
        std::string axis;                              
        PlantParams plant{};
        foundation::PidGains pid{0.0, 0.0, 0.0};
        foundation::Feedforward ff{0.0, 1.0, 0.0};   
        double baseline_ise{0.0};
        double tuned_ise{0.0};
        bool accepted{false};
        double velocity_command_gain{1.0};
        double measured_gain_before{0.0};
        double measured_gain_after{0.0};
        StepResponseData baseline_response{};
        StepResponseData tuned_response{};
    };

    struct VelocityTuneConfig
    {
        double step_duration_s{2.0};
        double step_command_frac{0.50};
        int sample_hz{200};
 
        // CHR gain scaling factors.
        double chr_kp_scale{0.3};
        double chr_ki_scale{0.6};
        double chr_kd_scale{0.15};
 
        // Rejection thresholds.
        double min_response_frac{0.10};
        double low_dead_time_ratio{0.05};
        double low_dead_time_gain_tol{0.15};
 
        // ---- MCU-chassis velocity-command-gain calibration ----
        double gain_min{0.3};
        double gain_max{3.0};
        double republish_settle_s{0.4};
        double steady_state_frac{0.5};
    };
 
    // ========================================================================
    // Phase 3 — MotionTuner types
    // ========================================================================

    struct MotionTuneResult
    {
        std::string param_name;
        double initial_kp{0.0};
        double initial_kd{0.0};
        double final_kp{0.0};
        double final_kd{0.0};
        double initial_score{0.0};
        double final_score{0.0};
        int iterations{0};
    };

    struct MotionTuneConfig
    {
        int sample_hz{100};                    
        double settle_s{0.5};                  
        int max_iterations{6};                 
        double initial_delta_frac{0.25};       
        double min_delta{0.01};                
        double delta_shrink{0.5};              
 
        // Trial parameters.
        // Every linear trial drives a FIXED distance (not speed-derived) so the
        // robot reliably uses the ~1 m runway the operator cleared. Forward to
        // linear_test_distance_m (recorded for scoring), then a closed return to
        // the start; repeat. Decoupled from max_velocity on purpose — a 0.2 m/s
        // chassis would otherwise only ever test ~0.2 m and never characterise
        // settle/overshoot at a realistic distance.
        double linear_test_distance_m{0.90};   
        double turn_test_angle_deg{90.0};      
        double motion_timeout_s{10.0};         
        double min_timeout_s{4.0};             
        double stuck_timeout_s{1.5};           
        double stuck_linear_progress_m{0.03};  
        double stuck_angular_progress_deg{8.0};
        double primary_speed_scale{1.0};       
        double min_linear_distance_m{0.40};    
        double max_linear_distance_m{1.00};    
 
        // Scoring weights.
        double score_settle_weight{1.0};
        double score_overshoot_weight{10.0};
        double score_error_weight{5.0};
        double score_timeout_penalty{50.0};
        double score_constraint_breach_base{25.0};
 
        // Soft limits for constraint-aware scoring.
        double linear_overshoot_soft_m{0.010};
        double linear_final_error_soft_m{0.010};
        double turn_overshoot_soft_rad{0.05236};   
        double turn_final_error_soft_rad{0.03491}; 
        double score_linear_overshoot_breach_per_m{2000.0};
        double score_linear_error_breach_per_m{1000.0};
        double score_turn_overshoot_breach_per_rad{400.0};
        double score_turn_error_breach_per_rad{250.0};
    };
 
    // ========================================================================
    // Phase 4 — FirmwarePidTuner types
    // ========================================================================

    struct FirmwarePidConfig
    {
        int raw_pwm_percent{70};
        double step_fraction{0.5};      
        double step_duration_s{5.0};
        double decel_duration_s{1.0};   
        int    sample_hz{200};          
        double chr_kp_scale{0.6};
        double chr_ki_scale{0.6};
        double chr_kd_scale{0.3};
        double min_response_frac{0.10}; 
        int identification_trials{3};
        int validation_trials{3};
        double accept_improvement_frac{0.02};
        std::string csv_dir{};
 
        // --------------------------------------------------------------------
        // ControlTheory differential-evolution gain computation.
        //
        // When use_control_theory is true the per-motor candidate gains are
        // computed by (1) fitting a PT1 plant to the raw-PWM step response via
        // DE system-identification, then (2) DE-optimizing physical firmware
        // kp/ki/kd against the simulated closed loop using the dt-EXPLICIT
        // ControlElementPID (which mirrors STM32 pid.c). The CHR heuristic is
        // kept only as a fallback when the DE fit fails.
        //
        // Scales:
        //  * plant K is BEMF-units-per-%PWM (tens-to-hundreds for the wombat
        //    motors at ~70% PWM), hence the large de_plant_max default.
        //  * gains are physical firmware kp/ki/kd (per-second for ki/kd).
        //  * de_max_overshoot is in output BEMF units.
        //  * de_sim_horizon_s sets the internal sim dt = horizon / 5000
        //    (default 5 s -> 1 ms). With the dt-explicit PID the optimized
        //    gains are physical, so the sim dt need not equal the firmware
        //    rate; the horizon only needs to cover settling.
        // --------------------------------------------------------------------
        bool          use_control_theory{true};
        int           de_plant_steps{80};
        double        de_plant_min{0.0001};
        double        de_plant_max{5000.0};
        int           de_pid_steps{60};
        double        de_gain_max{10.0};
        double        de_sim_horizon_s{5.0};
        double        de_max_overshoot{50.0};
        double        de_weight_ctrl{0.0};
        double        de_weight_overshoot{1.0};
        // Actuator saturation modeled in the closed-loop sim: the simulated PID
        // output and integral term are clamped to +/-de_output_max, matching the
        // firmware MOTOR_MAX_DUTYCYCLE (399). Without this the unbounded sim
        // output produces fictional overshoot and DE picks uselessly small gains.
        double        de_output_max{399.0};
        std::uint32_t de_seed{0xC0FFEEu};
    };

    struct FirmwarePidResult
    {
        int         motor_port{-1};
        PlantParams plant{};
        float       kp{0.0f};
        float       ki{0.0f};
        float       kd{0.0f};
        double      baseline_ise{0.0};
        double      tuned_ise{0.0};
        bool        accepted{false};
        std::string pid_method{"chr"};
        double      plant_K{0.0};
        double      plant_T{0.0};
    };
 
    // ========================================================================
    // Phase 5 — StaticFrictionMeasurer types
    // ========================================================================

    struct StaticFrictionConfig
    {
        int start_pct{2};          
        int max_pct{50};           
        int step_pct{1};           
        int dwell_ms{50};          
        int samples_per_step{5};   
        int motion_threshold{3};   
    };

    struct StaticFrictionSample
    {
        int pwm_pct{0};      
        int median_bemf{0};  
    };

    struct StaticFrictionResult
    {
        int  motor_port{-1};
        int  ks_positive_pct{0};
        int  ks_negative_pct{0};
        int  ks_avg_pct{0};
        bool measured{false};
        std::vector<StaticFrictionSample> forward_sweep{};
        std::vector<StaticFrictionSample> reverse_sweep{};
        int motion_threshold{0};
    };
 
    // ========================================================================
    // Phase 6 — VelLpfTuner types
    // ========================================================================

    struct VelLpfConfig
    {
        double alpha_min{0.05};        
        double alpha_max{0.95};        
        double alpha_step{0.05};       
        double measure_duration_s{1.0};
        int    sample_hz{200};         
        double noise_weight{1.0};      
        double lag_weight{0.5};        
        int    spin_percent{40};
        double settle_s{0.5};          
    };

    struct VelLpfSweepPoint
    {
        double alpha{0.0};
        double variance{0.0};        
        double lag_change_rate{0.0}; 
        double score{0.0};           
    };

    struct VelLpfResult
    {
        int    motor_port{-1};
        double initial_alpha{0.5};
        double tuned_alpha{0.5};
        double min_score{0.0};
        bool   applied{false};
        std::vector<int> raw_bemf{};
        std::vector<VelLpfSweepPoint> sweep{};
    };
 
    // ========================================================================
    // Phase 7 — ToleranceDeriver types
    // ========================================================================

    struct ToleranceConfig
    {
        double margin_factor{1.5};
        // Floor kept at 2 mm so tolerance autotune can't clamp the completion
        // band back above the uniform 2 mm target (motion_config.hpp /
        // executor DISTANCE_TOL_M).
        double min_distance_tolerance_m{0.002};
        double max_distance_tolerance_m{0.050};
        double min_angle_tolerance_rad{0.017};
        double max_angle_tolerance_rad{0.087};
    };

    struct ToleranceResult
    {
        double derived_distance_tolerance_m{0.002};
        double derived_angle_tolerance_rad{0.035};
        bool   distance_updated{false};
        bool   angle_updated{false};
    };
 
    // ========================================================================
    // Phase 8 — EncoderCalibrator types
    // ========================================================================

    struct EncoderCalConfig
    {
        double angular_velocity_rad_s{1.0};
        double total_angle_rad{4.0 * 3.14159265358979}; 
        double settle_s{0.5};
        int    sample_hz{100};
        double min_required_angle_rad{3.14159};         
    };

    struct EncoderCalResult
    {
        std::array<double, 4> ticks_to_rad{};    
        std::array<double, 4> scale_factors{};   
        double                imu_total_angle_rad{0.0};
        double                odom_total_angle_rad{0.0};
        bool                  success{false};
        std::string           failure_reason{};
    };
 
} // namespace libstp::autotune