auto_tuner.hpp

Go to the documentation of this file.

#pragma once
 
#include <functional>
#include <map>
#include <optional>
#include <string>
#include <vector>
 
#include "autotune/types.hpp"
#include "drive/drive.hpp"
#include "hal/IMotor.hpp"
#include "hal/odometry.hpp"
#include "motion/motion_config.hpp"
 
namespace libstp::autotune
{
    struct AutoTuneConfig
    {
        // Phase enable flags (in execution order).
        bool tune_vel_lpf{true};
        bool tune_static_friction{true};
        bool tune_firmware_pid{false};
        bool tune_encoder_cal{true};
        bool tune_characterize{true};
        bool tune_velocity{true};
        bool tune_motion{true};
        bool tune_tolerances{true};
 
        // Per-phase configurations.
        VelLpfConfig         vel_lpf_cfg{};
        StaticFrictionConfig static_friction_cfg{};
        FirmwarePidConfig    firmware_pid_cfg{};
        EncoderCalConfig     encoder_cal_cfg{};
        CharacterizeConfig   characterize_cfg{};
        VelocityTuneConfig   velocity_cfg{};
        MotionTuneConfig     motion_cfg{};
        ToleranceConfig      tolerance_cfg{};
 
        // Axes / parameters per phase (empty → auto-fill from kinematics support).
        std::vector<std::string> characterize_axes;
        std::vector<std::string> velocity_axes;
        std::vector<std::string> motion_params;
 
        // Map of axis name → max velocity (m/s or rad/s) for the velocity phase.
        // If empty, populated from the characterize phase results (or sensible
        // defaults when characterize is disabled).
        std::map<std::string, double> max_velocities;
 
        // Optional pre-supplied max-BEMF speeds for firmware PID tuning.
        // If unset, the AutoTuner runs a short power sweep to estimate them.
        std::optional<std::map<int, int>> max_bemf_speeds{};
    };

    struct AutoTuneResult
    {
        std::map<int, VelLpfResult>             vel_lpf{};
        std::map<int, StaticFrictionResult>     static_friction{};
        std::map<int, FirmwarePidResult>        firmware_pid{};
        EncoderCalResult                        encoder_cal{};
        std::map<std::string, AxisResult>       characterize{};
        std::map<std::string, VelocityTuneResult> velocity{};
        std::map<std::string, MotionTuneResult> motion{};
        ToleranceResult                         tolerances{};
 
        bool vel_lpf_ran{false};
        bool static_friction_ran{false};
        bool firmware_pid_ran{false};
        bool encoder_cal_ran{false};
        bool characterize_ran{false};
        bool velocity_ran{false};
        bool motion_ran{false};
        bool tolerances_ran{false};
    };

    class AutoTuner
    {
    public:
        using ConfirmCallback = std::function<void(const std::string& phase_key)>;
 
        AutoTuner(drive::Drive&                  drive,
                  odometry::IOdometry&           odometry,
                  motion::UnifiedMotionPidConfig& motion_pid_config);

        void setConfirmCallback(ConfirmCallback cb);

        AutoTuneResult tuneAll(const AutoTuneConfig& cfg);
 
        // --------------------------------------------------------------
        // Individual phases — usable standalone from Python wrappers.
        // --------------------------------------------------------------
 
        std::map<int, VelLpfResult>
        runVelLpfPhase(const VelLpfConfig& cfg);
 
        std::map<int, StaticFrictionResult>
        runStaticFrictionPhase(const StaticFrictionConfig& cfg);
 
        std::map<int, FirmwarePidResult>
        runFirmwarePidPhase(const FirmwarePidConfig& cfg,
                            std::optional<std::map<int, int>> max_bemf_speeds = std::nullopt);
 
        EncoderCalResult
        runEncoderCalibrationPhase(const EncoderCalConfig& cfg);
 
        std::map<std::string, AxisResult>
        runCharacterizePhase(const std::vector<std::string>& axes,
                             const CharacterizeConfig& cfg);
 
        std::map<std::string, VelocityTuneResult>
        runVelocityPhase(const std::vector<std::string>& axes,
                         const std::map<std::string, double>& max_velocities,
                         const VelocityTuneConfig& cfg);
 
        std::map<std::string, MotionTuneResult>
        runMotionPhase(const std::vector<std::string>& params,
                       const MotionTuneConfig& cfg);
 
        ToleranceResult
        runTolerancesPhase(const std::map<std::string, MotionTuneResult>& motion_results,
                           const MotionTuneConfig& motion_cfg,
                           const ToleranceConfig& cfg);
 
    private:
        void confirm(const std::string& phase_key);

        std::vector<hal::motor::IMotor*> motors();

        std::optional<std::map<int, int>> estimateMaxBemfSpeeds();

        bool bemfAvailable();
 
        drive::Drive&                   drive_;
        odometry::IOdometry&            odometry_;
        motion::UnifiedMotionPidConfig& motion_pid_config_;
        ConfirmCallback                 confirm_cb_{};
    };
 
} // namespace libstp::autotune