458 lines
17 KiB
C#
458 lines
17 KiB
C#
using RobotNet10.CANOpen.CiA402.Enums;
|
|
using RobotNet10.RobotApp.Motion;
|
|
using RobotNet10.RobotApp.Services.ConfigManager;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
|
|
|
|
|
|
|
|
/// <summary>
|
|
/// Configuration cho signal processing
|
|
/// </summary>
|
|
public class VelocitySignalProcessingConfig
|
|
{
|
|
/// <summary>
|
|
/// Hệ số lọc cho encoder velocity
|
|
/// Giá trị nhỏ (0.1-0.2): Smooth nhưng lag
|
|
/// Giá trị lớn (0.3-0.4): Responsive nhưng nhiễu
|
|
/// </summary>
|
|
public double AlphaFilter { get; set; } = 0.3;
|
|
|
|
/// <summary>
|
|
/// Ngưỡng phát hiện encoder nhiễu (m/s)
|
|
/// Nếu thay đổi vận tốc > threshold trong 1 cycle → có thể nhiễu
|
|
/// </summary>
|
|
public double NoiseThreshold { get; set; } = 0.5;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Configuration cho velocity estimator
|
|
/// </summary>
|
|
public class VelocityEstimatorConfig
|
|
{
|
|
// Blend ratio limits
|
|
public double MinBlendRatio { get; set; } = 0.15f;
|
|
public double MaxBlendRatio { get; set; } = 0.8f;
|
|
public double DefaultBlendRatio { get; set; } = 0.6;
|
|
|
|
// Adaptive blending thresholds
|
|
public double GoodTrackingThreshold { get; set; } = 0.12f; // < 10% error
|
|
public double ModerateTrackingThreshold { get; set; } = 0.3; // < 30% error
|
|
|
|
// Blend ratios for different tracking qualities
|
|
public double GoodTrackingBlend { get; set; } = 0.7;
|
|
public double ModerateTrackingBlend { get; set; } = 0.5;
|
|
public double PoorTrackingBlend { get; set; } = 0.25f;
|
|
|
|
// Model confidence decay
|
|
public double ConfidenceDecayRate { get; set; } = 0.95f;
|
|
public double MinConfidence { get; set; } = 0.3;
|
|
}
|
|
|
|
public class VelocityController(IInverseKinematics InverseKinematic,
|
|
OdometryService odometryService,
|
|
IRobotConfiguration RobtoConfiguration,
|
|
INavigationConfig NavigationConfig,
|
|
ILogger<VelocityController> _logger) : IVelocityController
|
|
{
|
|
public (double Linear, double Angular) ActualVelocity => GetCurrentVel();
|
|
public (double Linear, double Angular) RawVelocity => GetRawCurrentVel();
|
|
|
|
// vận tốc tính toán m/s và rad/s đối với vận tốc góc
|
|
private double _rightVelCmd = 0;
|
|
private double _leftVelCmd = 0;
|
|
private double _oldRightVel = 0;
|
|
private double _oldLeftVel = 0;
|
|
|
|
private MotorDynamicsConfig _motorDynamicsConifg = new();
|
|
private MotorDynamicsModel _motorDynamicsModel = new();
|
|
private VelocityEstimatorConfig _estimatorConfig = new();
|
|
public PurePursuitConfig _purePursuitConfig = new();
|
|
private VelocitySignalProcessingConfig _signalConfig = new();
|
|
private readonly CircularBuffer<double> _predictionErrors = new(20);
|
|
|
|
private double _currentConfidence = 1.0;
|
|
private readonly double wheelBase = RobtoConfiguration.GetRobotPhysicalConfig().WheelBase;
|
|
private int _ensureIKReadyCounter = 0; // Counter for logging throttling
|
|
|
|
public void SetVelocity(double linearVel, double angularVel)
|
|
{
|
|
// InverseKinematic.SetVelocity not available - commented out
|
|
// InverseKinematic.SetVelocity(new()
|
|
// {
|
|
// Linear = new(){
|
|
// X = linearVel,
|
|
// Y = 0,
|
|
// },
|
|
// Angular = new(){
|
|
// Z = angularVel,
|
|
// },
|
|
// });
|
|
_leftVelCmd = linearVel - (wheelBase / 2) * angularVel;
|
|
_rightVelCmd = linearVel + (wheelBase / 2) * angularVel;
|
|
}
|
|
public (double linearVel, double angularVel) GetRawCurrentVel()
|
|
{
|
|
try
|
|
{
|
|
var odom = odometryService.CurrentOdometry;
|
|
double vActual = odom.Twist.Twist.Linear.X;
|
|
double omegaActual = odom.Twist.Twist.Angular.Z;
|
|
return (vActual, omegaActual);
|
|
}
|
|
catch { return (0, 0); }
|
|
}
|
|
|
|
public (double linearVel, double angularVel) GetCurrentVel()
|
|
{
|
|
try
|
|
{
|
|
var odom = odometryService.CurrentOdometry;
|
|
var vActual = odom.Twist.Twist.Linear.X;
|
|
var omegaActual = odom.Twist.Twist.Angular.Z;
|
|
|
|
// Convert linear/angular back to left/right wheel velocities for the estimator
|
|
_oldLeftVel = vActual - (wheelBase / 2) * omegaActual;
|
|
_oldRightVel = vActual + (wheelBase / 2) * omegaActual;
|
|
|
|
return Estimate(_oldLeftVel, _oldRightVel, _leftVelCmd, _rightVelCmd, wheelBase);
|
|
}
|
|
catch { return (0, 0); }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Exponential Moving Average (EMA) Low-Pass Filter
|
|
/// </summary>
|
|
/// <param name="newValue">Giá trị mới từ sensor</param>
|
|
/// <param name="oldValue">Giá trị đã lọc trước đó</param>
|
|
/// <param name="alpha">Hệ số lọc (0-1). Càng nhỏ càng smooth, càng lớn càng responsive</param>
|
|
/// <returns>Giá trị sau khi lọc</returns>
|
|
private static double LowPassFilter(double newValue, double oldValue, double alpha)
|
|
{
|
|
// Validate alpha
|
|
if (alpha < 0 || alpha > 1)
|
|
{
|
|
throw new ArgumentException("Alpha must be between 0 and 1", nameof(alpha));
|
|
}
|
|
|
|
return alpha * newValue + (1.0 - alpha) * oldValue;
|
|
}
|
|
|
|
/// <summary>
|
|
/// MAIN FUNCTION: Estimate velocity
|
|
/// </summary>
|
|
private (double linearVel, double angularVel) Estimate(
|
|
double vLeftActual, // Từ encoder (filtered)
|
|
double vRightActual, // Từ encoder (filtered)
|
|
double vLeftCmdPrev, // Command từ cycle trước
|
|
double vRightCmdPrev, // Command từ cycle trước
|
|
double wheelbase)
|
|
{
|
|
// 1. Tính vận tốc actual (linear & angular)
|
|
double vActual = (vLeftActual + vRightActual) / 2.0;
|
|
double omegaActual = (vRightActual - vLeftActual) / wheelbase;
|
|
|
|
// 2. Tính vận tốc command từ cycle trước
|
|
double vCmdPrev = (vLeftCmdPrev + vRightCmdPrev) / 2.0;
|
|
double omegaCmdPrev = (vRightCmdPrev - vLeftCmdPrev) / wheelbase;
|
|
|
|
// 3. Tính prediction horizon
|
|
double predictionHorizon = CalculatePredictionHorizon(vActual);
|
|
|
|
// 4. Predict velocity cho từng bánh
|
|
double vLeftPredicted = _motorDynamicsModel.PredictVelocity(
|
|
vLeftCmdPrev,
|
|
vLeftActual,
|
|
predictionHorizon
|
|
);
|
|
|
|
double vRightPredicted = _motorDynamicsModel.PredictVelocity(
|
|
vRightCmdPrev,
|
|
vRightActual,
|
|
predictionHorizon
|
|
);
|
|
|
|
// 5. Tính linear & angular predicted
|
|
double vPredicted = (vLeftPredicted + vRightPredicted) / 2.0;
|
|
double omegaPredicted = (vRightPredicted - vLeftPredicted) / wheelbase;
|
|
|
|
// 6. Update model confidence
|
|
UpdateModelConfidence(vPredicted, vActual);
|
|
|
|
// 7. Tính tracking error
|
|
double linearErr = CalculateLinearTrackingError(vCmdPrev, vActual);
|
|
double angularErr = CalculateAngularTrackingError(omegaCmdPrev, omegaActual);
|
|
double combinedTrackingError = 0.65f * linearErr + 0.35f * angularErr;
|
|
|
|
// 8. Calculate adaptive blend ratio
|
|
double blendRatio = CalculateAdaptiveBlendRatio(
|
|
combinedTrackingError,
|
|
_currentConfidence
|
|
);
|
|
|
|
// 9. Blend predicted và actual
|
|
var vHybrid = (blendRatio * vPredicted) + ((1.0 - blendRatio) * vActual);
|
|
double omegaHybrid = blendRatio * omegaPredicted + (1.0 - blendRatio) * omegaActual;
|
|
|
|
// 10. Return result
|
|
return (vHybrid, omegaHybrid);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Tính prediction horizon dựa vào lookahead distance
|
|
/// </summary>
|
|
private double CalculatePredictionHorizon(double vActual)
|
|
{
|
|
// Lookahead distance
|
|
double lookahead = _purePursuitConfig.LookaheadMin + _purePursuitConfig.Kdd * Math.Abs(vActual);
|
|
|
|
lookahead = Math.Clamp(lookahead, _purePursuitConfig.LookaheadMin, _purePursuitConfig.LookaheadMax);
|
|
|
|
// Prediction time = lookahead / velocity
|
|
// Nếu vận tốc quá nhỏ, dùng một giá trị minimum
|
|
double predictionTime = lookahead / Math.Max(Math.Abs(vActual), 0.1);
|
|
|
|
// Giới hạn prediction time (không nên quá xa)
|
|
predictionTime = Math.Clamp(predictionTime, 0.1, 2.0);
|
|
|
|
return predictionTime;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Tính linear velocity tracking error (normalized)
|
|
/// </summary>
|
|
private static double CalculateLinearTrackingError(double vCmd, double vActual)
|
|
{
|
|
double error = Math.Abs(vCmd - vActual);
|
|
double normalizedError = error / Math.Max(Math.Abs(vCmd), 0.1);
|
|
return normalizedError;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Tính angular velocity tracking error (normalized)
|
|
/// </summary>
|
|
private static double CalculateAngularTrackingError(double oCmd, double oActual)
|
|
{
|
|
double error = Math.Abs(oCmd - oActual);
|
|
double normalizedError = error / Math.Max(Math.Abs(oCmd), 0.05f);
|
|
return normalizedError;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Update model confidence dựa trên prediction accuracy
|
|
/// </summary>
|
|
private void UpdateModelConfidence(double vPredictedPrev, double vActualNow)
|
|
{
|
|
// Prediction error từ cycle trước
|
|
double predError = Math.Abs(vPredictedPrev - vActualNow) / Math.Max(Math.Abs(vActualNow), 0.1);
|
|
|
|
_predictionErrors.Add(predError);
|
|
|
|
// Tính confidence dựa trên average error
|
|
if (_predictionErrors.Count > 0)
|
|
{
|
|
double avgError = _predictionErrors.Average();
|
|
|
|
// Confidence = 1 - avgError (capped)
|
|
double newConfidence = Math.Clamp(1.0 - avgError, 0.0, 1.0);
|
|
|
|
// Smooth update với decay
|
|
_currentConfidence = _estimatorConfig.ConfidenceDecayRate * _currentConfidence + (1.0 - _estimatorConfig.ConfidenceDecayRate) * newConfidence;
|
|
|
|
_currentConfidence = Math.Max(_currentConfidence, _estimatorConfig.MinConfidence);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate adaptive blend ratio
|
|
/// </summary>
|
|
private double CalculateAdaptiveBlendRatio(
|
|
double trackingError,
|
|
double modelConfidence)
|
|
{
|
|
double alpha;
|
|
|
|
// Factor 1: Tracking error
|
|
if (trackingError < _estimatorConfig.GoodTrackingThreshold)
|
|
{
|
|
// Motor tracking tốt → tin prediction nhiều
|
|
alpha = _estimatorConfig.GoodTrackingBlend;
|
|
}
|
|
else if (trackingError < _estimatorConfig.ModerateTrackingThreshold)
|
|
{
|
|
// Moderate error → balanced
|
|
alpha = _estimatorConfig.ModerateTrackingBlend;
|
|
}
|
|
else
|
|
{
|
|
// Poor tracking (slip/overload) → tin actual nhiều
|
|
alpha = _estimatorConfig.PoorTrackingBlend;
|
|
}
|
|
|
|
// Factor 2: Model confidence
|
|
// Nếu model không chính xác, giảm blend ratio
|
|
alpha *= modelConfidence;
|
|
|
|
// Clamp trong khoảng cho phép
|
|
alpha = Math.Clamp(alpha, _estimatorConfig.MinBlendRatio, _estimatorConfig.MaxBlendRatio);
|
|
|
|
return alpha;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reset estimator state
|
|
/// </summary>
|
|
public void Reset()
|
|
{
|
|
_predictionErrors.Clear();
|
|
_currentConfidence = 1.0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get current model confidence
|
|
/// </summary>
|
|
public double GetModelConfidence()
|
|
{
|
|
return _currentConfidence;
|
|
}
|
|
|
|
public void LoadConfig()
|
|
{
|
|
_motorDynamicsConifg = NavigationConfig.GetMotorDynamicsConfig();
|
|
_motorDynamicsModel = new(_motorDynamicsConifg);
|
|
_estimatorConfig = NavigationConfig.GetVelocityEstimatorConfig();
|
|
_purePursuitConfig = NavigationConfig.GetPurepursuitConfig();
|
|
_signalConfig = NavigationConfig.GetVelocitySignalProcessingConfig();
|
|
}
|
|
|
|
public void SetAcceleration(double acc)
|
|
{
|
|
// SetAcceleration not available - commented out
|
|
// InverseKinematic.SetAcceleration(acc);
|
|
}
|
|
|
|
public void SetDeceleration(double dec)
|
|
{
|
|
// SetDeceleration not available - commented out
|
|
// InverseKinematic.SetDeceleration(dec);
|
|
}
|
|
|
|
public bool EnsureInverseKinematicsReady(CancellationToken cancellationToken)
|
|
{
|
|
try
|
|
{
|
|
// Increment counter for logging throttling
|
|
_ensureIKReadyCounter++;
|
|
|
|
// Check if need to reset fault first
|
|
// Note: DifferentialDrive doesn't expose IsFaulted, so we try FaultReset if not enabled
|
|
if (!InverseKinematic.IsOperationEnabled)
|
|
{
|
|
// Try fault reset first (in case it's in fault state)
|
|
InverseKinematic.FaultReset();
|
|
PreciseDelay(200, cancellationToken);
|
|
}
|
|
|
|
// Check if IInverseKinematics is in OperationEnabled state
|
|
if (!InverseKinematic.IsOperationEnabled)
|
|
{
|
|
// Auto-enable IInverseKinematics through state transitions
|
|
// Enable() is a convenience method that automatically transitions through all states
|
|
int maxAttempts = 3;
|
|
int attemptDelay = 300; // ms
|
|
|
|
for (int i = 0; i < maxAttempts && !InverseKinematic.IsOperationEnabled; i++)
|
|
{
|
|
InverseKinematic.Enable();
|
|
PreciseDelay(attemptDelay, cancellationToken);
|
|
}
|
|
|
|
// Check if enabled successfully
|
|
if (!InverseKinematic.IsOperationEnabled)
|
|
{
|
|
// Log only once every 10 times to avoid spam
|
|
if (_ensureIKReadyCounter % 10 == 0)
|
|
{
|
|
_logger.LogWarning("IInverseKinematics is not in OperationEnabled state. Cannot send velocity.");
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Check and set operation mode to ProfileVelocity (synchronous)
|
|
try
|
|
{
|
|
// GetOperationMode/SetOperationMode not available - commented out
|
|
// OperationMode currentMode = InverseKinematic.GetOperationMode();
|
|
// if (currentMode != OperationMode.ProfileVelocity)
|
|
// {
|
|
// InverseKinematic.SetOperationMode(OperationMode.ProfileVelocity);
|
|
// }
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Log only once every 10 times to avoid spam
|
|
if (_ensureIKReadyCounter % 10 == 0)
|
|
{
|
|
_logger.LogWarning(ex, "Error checking/setting operation mode");
|
|
}
|
|
// Continue anyway to prevent blocking
|
|
}
|
|
|
|
return true;
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
throw;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Log only once every 10 times to avoid spam
|
|
if (_ensureIKReadyCounter % 10 == 0)
|
|
{
|
|
_logger.LogError(ex, "Error ensuring IInverseKinematics ready");
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Precise synchronous delay using Thread.Sleep for longer delays and SpinWait for short delays
|
|
/// This ensures accurate timing for the update loop without async overhead
|
|
/// </summary>
|
|
private static void PreciseDelay(int milliseconds, CancellationToken cancellationToken)
|
|
{
|
|
if (milliseconds <= 0)
|
|
return;
|
|
|
|
if (milliseconds > 1)
|
|
{
|
|
// Use Thread.Sleep for longer delays (synchronous, more precise in dedicated thread)
|
|
// Check cancellation periodically during sleep
|
|
var sleepStart = DateTime.UtcNow;
|
|
while ((DateTime.UtcNow - sleepStart).TotalMilliseconds < milliseconds)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
return;
|
|
|
|
var remaining = milliseconds - (int)(DateTime.UtcNow - sleepStart).TotalMilliseconds;
|
|
if (remaining > 0)
|
|
{
|
|
Thread.Sleep(Math.Min(remaining, 10)); // Sleep in 10ms chunks to check cancellation
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Use SpinWait for very short delays to maintain precise timing
|
|
var spinWait = new SpinWait();
|
|
for (int i = 0; i < 10; i++)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
break;
|
|
spinWait.SpinOnce();
|
|
}
|
|
}
|
|
}
|
|
}
|