velocity_tune.hpp

Go to the documentation of this file.

#pragma once
 
#include <map>
#include <optional>
#include <string>
#include <vector>
 
#include "autotune/types.hpp"
#include "drive/drive.hpp"
#include "hal/odometry.hpp"
 
namespace libstp::autotune
{
    class VelocityTuner
    {
    public:
        VelocityTuner(drive::Drive& drive, odometry::IOdometry& odometry);

        [[nodiscard]] VelocityTuneResult tuneAxis(
            const std::string&       axis,
            double                   max_velocity,
            const VelocityTuneConfig& cfg) const;

        [[nodiscard]] std::map<std::string, VelocityTuneResult> tune(
            const std::vector<std::string>&        axes,
            const std::map<std::string, double>&   max_velocities,
            const VelocityTuneConfig&              cfg) const;
 
    private:
        [[nodiscard]] StepResponseData runStepResponse(
            const std::string& axis,
            double             command,
            double             duration_s,
            int                sample_hz) const;

        [[nodiscard]] std::optional<PlantParams> identifyPlantInflection(
            const StepResponseData& data,
            double                  command) const;

        [[nodiscard]] PlantParams identifyPlantRiseTime(
            const StepResponseData& data,
            double                  command) const;

        [[nodiscard]] foundation::PidGains computeChrGains(
            const PlantParams&       plant,
            const VelocityTuneConfig& cfg) const;

        [[nodiscard]] static double computeIse(const StepResponseData& data);

        [[nodiscard]] static double tailMeanAbs(const StepResponseData& data);

        [[nodiscard]] static double peakAbs(const StepResponseData& data);
 
        drive::Drive&        drive_;
        odometry::IOdometry& odometry_;
    };
 
} // namespace libstp::autotune