QUYVN/Denso
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:31:37 +07:00
commit 899c7c637d
1939 changed files with 641750 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

359
srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/Configs.cs Normal file
View File

@@ -0,0 +1,359 @@
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// PID Controller configuration
/// </summary>
public class PIDConfig
{
public double Kp { get; set; }
public double Ki { get; set; }
public double Kd { get; set; }
/// <summary>
/// Integral chỉ tích lũy khi |error| &lt;= IntegralZone.
/// Giá trị 0 = không giới hạn (integral luôn tích lũy).
/// </summary>
public double IntegralZone { get; set; }
}
/// <summary>
/// Motor Dynamics configuration
/// </summary>
public class MotorDynamicsConfig
{
/// <summary>
/// Time constant (τ) - thời gian để motor đạt 63.2% của target velocity
/// Đơn vị: giây (s)
/// Typical: 0.1 - 0.5s cho DC motor với driver PID
/// </summary>
public double Tau { get; set; }
/// <summary>
/// Pure delay (δ) - độ trễ trước khi motor bắt đầu phản ứng
/// Đơn vị: giây (s)
/// Bao gồm: communication delay + driver processing
/// Typical: 0.02 - 0.1s
/// </summary>
public double Delta { get; set; }
}
/// <summary>
/// Stanley Controller configuration
/// Path tracking using cross-track error and heading error
/// </summary>
public class StanleyConfig
{
#region Core Stanley Parameters
/// <summary>
/// Cross-track error gain (K)
/// Default: 2.5
///
/// Meaning: How aggressively to correct lateral position error
/// Formula: δ = ψ + arctan(K × e / (v + Ks))
///
/// ↑ Increase (3.0-5.0):
/// ✓ Faster correction of cross-track error
/// ✓ Tighter path following
/// ✗ May cause oscillation
/// ✗ Less smooth on noisy paths
///
/// ↓ Decrease (1.5-2.0):
/// ✓ Smoother motion
/// ✓ Less oscillation
/// ✗ Slower error correction
/// ✗ Larger cross-track error
///
/// Tuning Tips:
/// - Start: 2.5 for general use
/// - High precision: 3.0-4.0
/// - Smooth priority: 1.5-2.0
/// - Check stability by observing steering oscillation
/// </summary>
public double K { get; set; } = 2.5;
/// <summary>
/// Softening constant (Ks) - meters/second
/// Default: 0.1 m/s
///
/// Meaning: Added to velocity denominator to prevent division by zero at low speeds
/// Formula: δ = ψ + arctan(K × e / (v + Ks))
///
/// ↑ Increase (0.15-0.2):
/// ✓ Less aggressive correction at low speed
/// ✓ Smoother motion when starting
/// ✗ Slower error correction at low speed
///
/// ↓ Decrease (0.05-0.08):
/// ✓ More responsive at low speed
/// ✗ May cause oscillation when slow
/// ✗ Risk of instability near zero velocity
///
/// Tuning Tips:
/// - Should be ~10% of typical operating velocity
/// - If robot oscillates when slow: increase to 0.15-0.2
/// - If too sluggish at startup: decrease to 0.05-0.08
/// </summary>
public double Ks { get; set; } = 0.1;
#endregion
#region Vehicle Parameters
/// <summary>
/// Wheelbase (L) - distance between front and rear axles (meters)
/// Default: 0.5m
///
/// Meaning: Distance from rear axle (robot center) to virtual front axle
/// Used to calculate front axle position and convert steering angle to angular velocity
///
/// IMPORTANT: Must match actual robot geometry
///
/// Formula: ω = (v × tan(δ)) / L
/// </summary>
public double WheelBase { get; set; } = 0.6;
/// <summary>
/// Maximum steering angle (radians)
/// Default: 0.5 rad (≈28.6°)
///
/// Meaning: Physical limit of equivalent steering angle
///
/// ↑ Increase (0.6-0.8 rad ≈ 34-46°):
/// ✓ Sharper turns possible
/// ✗ May exceed robot's turning capability
///
/// ↓ Decrease (0.3-0.4 rad ≈ 17-23°):
/// ✓ Safer, gentler turns
/// ✗ Cannot track sharp curves
///
/// Tuning Tips:
/// - Test robot's max practical turn rate
/// - Calculate: δ_max = arctan(L × ω_max / v_typical)
/// - Example: L=0.5m, ω_max=2rad/s, v=1m/s → δ_max = 0.785 rad (45°)
/// - Conservative: 0.4-0.5 rad
/// </summary>
public double MaxSteeringAngle { get; set; } = 0.5;
#endregion
#region Curvature Feedforward Parameters
/// <summary>
/// Enable curvature feedforward term
/// Default: true
///
/// Meaning: Add path curvature prediction to steering command
/// Formula: δ = ψ + arctan(K×e/(v+Ks)) + arctan(κ×L)
///
/// ✓ Enabled:
/// ✓ Better tracking on curved paths
/// ✓ Anticipates turns, less lag
/// ✗ Requires accurate path curvature
///
/// ✗ Disabled:
/// ✓ Simpler, more predictable
/// ✓ Works with rough path data
/// ✗ May lag on curves
///
/// Tuning Tips:
/// - Enable for smooth, well-defined paths
/// - Disable if path is noisy or has discontinuities
/// </summary>
public bool EnableCurvatureFeedforward { get; set; } = true;
/// <summary>
/// Curvature feedforward gain
/// Default: 1.0
///
/// Meaning: Scaling factor for curvature term
/// Full formula: δ = ψ + arctan(K×e/(v+Ks)) + KCurvatureFF × arctan(κ×L)
///
/// ↑ Increase (1.2-1.5):
/// ✓ More aggressive curve anticipation
/// ✓ Less lag on sharp turns
/// ✗ May overshoot on curves
///
/// ↓ Decrease (0.7-0.9):
/// ✓ Gentler curve following
/// ✗ More lag on curves
///
/// Tuning Tips:
/// - Start at 1.0
/// - If cutting corners: increase to 1.1-1.3
/// - If overshooting curves: decrease to 0.8-0.9
/// </summary>
public double KCurvatureFF { get; set; } = 1.0;
#endregion
#region Goal Approach Parameters
/// <summary>
/// Distance to goal to consider "reached" (meters)
/// Default: 0.05m (5cm)
///
/// Meaning: Stop criterion - when within this distance, goal is reached
///
/// ↑ Increase (0.08-0.1m):
/// ✓ Faster completion
/// ✗ Lower precision
///
/// ↓ Decrease (0.02-0.03m):
/// ✓ Higher precision
/// ✗ May never reach due to localization error
///
/// Tuning Tips:
/// - Must be ≥ 2× localization RMS error
/// - Typical: 0.03-0.05m
/// </summary>
public double GoalTolerance { get; set; } = 0.05;
/// <summary>
/// Heading tolerance at goal (degrees)
/// Default: 5.0°
///
/// Meaning: Acceptable heading error when reaching goal
///
/// Tuning Tips:
/// - Strict docking: 2-3°
/// - Normal navigation: 5-10°
/// </summary>
public double HeadingTolerance { get; set; } = 5.0;
/// <summary>
/// Distance to start increasing K gain near goal (meters)
/// Default: 1.0m
///
/// Meaning: When within this distance, K gain increases linearly
/// to improve tracking accuracy during final approach
///
/// ↑ Increase (1.5-2.0m):
/// ✓ Earlier tightening, smoother transition
/// ✗ May be too aggressive on long approach
///
/// ↓ Decrease (0.5-0.8m):
/// ✓ Only tighten very close to goal
/// ✗ Less time to correct errors
///
/// Tuning Tips:
/// - Should be larger than GoalTolerance × 10
/// - Typical: 0.8-1.5m
/// </summary>
public double GoalApproachDistance { get; set; } = 1.0;
/// <summary>
/// K gain multiplier at goal position
/// Default: 2.0 (K doubles when at goal)
///
/// Meaning: At goal, effective K = K × GoalGainMultiplier
/// Linearly interpolated from 1.0 at GoalApproachDistance to this value at goal
///
/// ↑ Increase (2.5-3.0):
/// ✓ Much tighter tracking near goal
/// ✗ Risk of oscillation
///
/// ↓ Decrease (1.3-1.5):
/// ✓ Gentler increase
/// ✗ Less improvement near goal
///
/// Tuning Tips:
/// - Start at 2.0
/// - If oscillating near goal: decrease to 1.5
/// - If still drifting: increase to 2.5
/// </summary>
public double GoalGainMultiplier { get; set; } = 2.0;
#endregion
#region Low Speed Control Parameters
/// <summary>
/// Velocity threshold below which direct angular control activates (m/s)
/// Default: 0.3 m/s
///
/// Meaning: Below this speed, bicycle model is blended with direct proportional control.
/// This prevents the angular velocity from collapsing to zero when the robot
/// decelerates near the goal.
///
/// Problem it solves:
/// Bicycle model: ω = v × tan(δ) / L
/// When v → 0, ω → 0, even if δ is large → robot cannot correct
///
/// ↑ Increase (0.4-0.5):
/// ✓ Direct control kicks in earlier
/// ✗ May feel less smooth at moderate speeds
///
/// ↓ Decrease (0.15-0.2):
/// ✓ Only activates at very low speed
/// ✗ May still drift at medium-low speeds
///
/// Tuning Tips:
/// - Should be close to NavigationConfig.MinLinearVelocity × 2-3
/// - Typical: 0.2-0.4 m/s
/// </summary>
public double LowSpeedThreshold { get; set; } = 0.3;
/// <summary>
/// Angular velocity gain for direct control at low speeds
/// Default: 1.5
///
/// Meaning: At zero speed, ω = LowSpeedAngularGain × steeringAngle
/// Ensures the robot can still correct heading/cross-track errors
/// when the bicycle model would produce near-zero angular velocity.
///
/// ↑ Increase (2.0-3.0):
/// ✓ Stronger correction at low speed
/// ✗ May oscillate near goal
///
/// ↓ Decrease (0.8-1.0):
/// ✓ Gentler low-speed correction
/// ✗ Slower error correction
///
/// Tuning Tips:
/// - Start at 1.5
/// - If oscillating at low speed: decrease to 1.0
/// - If not correcting fast enough: increase to 2.0
/// </summary>
public double LowSpeedAngularGain { get; set; } = 1.5;
#endregion
#region Angular Velocity Limit
/// <summary>
/// Maximum angular velocity during final approach (rad/s)
/// Default: 1.0 rad/s
///
/// Meaning: Clamps the angular velocity output of Stanley controller
/// to prevent excessive rotation near the goal.
///
/// ↑ Increase (1.5-2.0):
/// ✓ Faster heading correction
/// ✗ May overshoot or oscillate
///
/// ↓ Decrease (0.5-0.8):
/// ✓ Smoother, gentler rotation near goal
/// ✗ Slower heading correction
///
/// Tuning Tips:
/// - Should be ≤ robot's physical max angular velocity
/// - Typically lower than PurePursuit MaxAngularVelocity for smoother final approach
/// - Start at 1.0, decrease if robot oscillates near goal
/// </summary>
public double MaxAngularVelocity { get; set; } = 1.0;
#endregion
#region Path Resolution
/// <summary>
/// Waypoint spacing for path sampling (meters)
/// Default: 0.05m (5cm)
///
/// Meaning: Distance between interpolated path points
/// Same as PurePursuit.ResolutionSplit for consistency
/// </summary>
public double ResolutionSplit { get; set; } = 0.05;
#endregion
}