motion_tune.hpp

Go to the documentation of this file.

#pragma once
 
#include <map>
#include <mutex>
#include <string>
#include <tuple>
#include <vector>
#include <condition_variable>
#include <atomic>
 
#include "autotune/types.hpp"
#include "drive/drive.hpp"
#include "hal/odometry.hpp"
#include "motion/linear_motion.hpp"
#include "motion/motion_config.hpp"
 
namespace libstp::autotune
{
    class MotionTuner
    {
    public:
        MotionTuner(drive::Drive& drive,
                    odometry::IOdometry& odometry,
                    motion::UnifiedMotionPidConfig& pid_config);

        [[nodiscard]] std::map<std::string, MotionTuneResult> tune(
            const std::vector<std::string>& params,
            const MotionTuneConfig& cfg) const;

        [[nodiscard]] MotionTuneResult tuneParam(
            const std::string& param_name,
            const MotionTuneConfig& cfg) const;

        void requestPauseAfterTrial() const noexcept;

        void resume() const noexcept;

        [[nodiscard]] std::string pauseState() const;
 
    private:
        [[nodiscard]] std::tuple<double, double, double> runLinearTrial(
            motion::LinearAxis axis,
            double distance_m,
            double speed_scale,
            const MotionTuneConfig& cfg) const;

        [[nodiscard]] std::tuple<double, double, double> runTurnTrial(
            double angle_rad,
            double speed_scale,
            const MotionTuneConfig& cfg) const;

        void returnLinear(motion::LinearAxis axis, double home_heading_rad,
                          const MotionTuneConfig& cfg) const;

        void returnTurn(double home_heading_rad, const MotionTuneConfig& cfg) const;

        [[nodiscard]] double evaluateGains(
            const std::string& param_name,
            double kp,
            double kd,
            int trial_idx,
            const MotionTuneConfig& cfg) const;

        [[nodiscard]] double scoreMotionTrial(
            const std::string& param_name,
            double settle_time,
            double overshoot,
            double final_error,
            const MotionTuneConfig& cfg) const;

        [[nodiscard]] double linearTargetDistance(
            motion::LinearAxis axis,
            const MotionTuneConfig& cfg) const;

        [[nodiscard]] double trialTimeoutS(
            double distance_m,
            double max_velocity,
            double speed_scale,
            const MotionTuneConfig& cfg) const;

        void pauseIfRequested() const;
 
        drive::Drive& drive_;
        odometry::IOdometry& odometry_;
        motion::UnifiedMotionPidConfig& pid_config_;
        mutable std::atomic<bool> pause_requested_{false};
        mutable std::atomic<bool> paused_{false};
        mutable std::mutex pause_mutex_;
        mutable std::condition_variable pause_cv_;
    };
 
} // namespace libstp::autotune