motion_config.hpp

Go to the documentation of this file.

#pragma once
 
#include "foundation/pid.hpp"
 
namespace libstp::motion
{
    inline constexpr double kMotionEpsilon = 1e-6;

    struct AxisConstraints
    {
        double max_velocity{0.0};
        double acceleration{0.0};
        double deceleration{0.0};
 
        AxisConstraints() = default;
 
        AxisConstraints(double max_velocity, double acceleration, double deceleration)
            : max_velocity(max_velocity), acceleration(acceleration), deceleration(deceleration)
        {}
    };

    struct UnifiedMotionPidConfig
    {
        // Distance/Position PID (used for forward/backward distance, lateral distance)
        foundation::PidConfig distance{2.0, 0.0, 0.5, 10.0, 0.01, 0.3};
 
        // Heading/Angular PID (used for maintaining heading in drive/strafe, and for turning)
        foundation::PidConfig heading{2.0, 0.0, 0.3, 10.0, 0.01, 0.3};
 
        // Profiled PID velocity feedforward gain.
        // 1.0 = full feedforward from profile velocity (recommended for velocity-commanding systems).
        // 0.0 = pure PID on position error (WPILib-style, needs larger kP).
        double velocity_ff{1.0};
 
        // Saturation handling (speed-independent derating factors)
        double saturation_derating_factor{0.9};
        double saturation_min_scale{0.2};
        double saturation_recovery_rate{0.03};
 
        // Heading-specific saturation handling
        double heading_saturation_derating_factor{0.85};
        double heading_min_scale{0.25};
        double heading_recovery_rate{0.05};
 
        // Hysteresis parameters to prevent oscillation
        int saturation_hold_cycles{5};
        double saturation_recovery_threshold{0.95};
 
        // Tolerances
        // 2 mm: the motion must actually reach the goal. A looser band let the
        // profiled PID declare "done" ~1 cm short, hard-stop, and leave the
        // robot parked short of the target (the strafe-reversal stall). Kept in
        // lockstep with the executor's DISTANCE_TOL_M / progress eps (2 mm).
        double distance_tolerance_m{0.002};
        double angle_tolerance_rad{0.035};
 
        // Saturation heading thresholds
        double heading_saturation_error_rad{0.01};
        double heading_recovery_error_rad{0.005};
 
        // Max rate-of-change of the internal heading-hold omega output in
        // LinearMotion (rad/s^2). Prevents a motion that starts with an
        // inherited heading error from snapping to a saturated omega in its
        // first tick (visible yaw jerk + overshoot on high-inertia chassis).
        // 0 disables the limit (legacy behaviour). Does not affect the line
        // follow omega override, which runs its own controller.
        double heading_omega_slew{0.0};
 
        // Per-axis motion profile constraints, typically populated from
        // characterization data such as characterize_drive().
        AxisConstraints linear{};
        AxisConstraints lateral{};
        AxisConstraints angular{};
    };

    inline UnifiedMotionPidConfig defaultUnifiedMotionPidConfig()
    {
        return UnifiedMotionPidConfig{};
    }
}