1178 lines
42 KiB
C#
1178 lines
42 KiB
C#
using Appccelerate.StateMachine;
|
||
using Appccelerate.StateMachine.Machine;
|
||
using Microsoft.Extensions.Configuration;
|
||
using Microsoft.Extensions.Hosting;
|
||
using Microsoft.Extensions.Logging;
|
||
using RobotNet10.CANOpen.CiA402.Enums;
|
||
using RobotNet10.RobotApp.Devices;
|
||
using RobotNet10.RobotApp.Interfaces;
|
||
using RobotNet10.Shared.Geometry;
|
||
using System;
|
||
using System.Threading;
|
||
using System.Threading.Tasks;
|
||
|
||
namespace RobotNet10.RobotApp.Motion;
|
||
|
||
/// <summary>
|
||
/// States cho DifferentialDrive state machine - tương tự CiA402 DriveState
|
||
/// </summary>
|
||
public enum DifferentialDriveState
|
||
{
|
||
NotReadyToSwitchOn,
|
||
SwitchOnDisabled,
|
||
ReadyToSwitchOn,
|
||
SwitchedOn,
|
||
OperationEnabled,
|
||
QuickStopActive,
|
||
FaultReactionActive,
|
||
Fault
|
||
}
|
||
|
||
/// <summary>
|
||
/// Triggers cho DifferentialDrive state machine - tương tự CiA402 Controlword commands
|
||
/// </summary>
|
||
public enum DifferentialDriveTrigger
|
||
{
|
||
Shutdown, // Disable voltage
|
||
SwitchOn, // Switch on command
|
||
DisableVoltage, // Disable voltage command
|
||
QuickStop, // Quick stop command
|
||
EnableOperation, // Enable operation command
|
||
DisableOperation, // Disable operation command
|
||
FaultReset // Fault reset command
|
||
}
|
||
|
||
/// <summary>
|
||
/// DifferentialDrive service - điều khiển robot di chuyển bằng 2 bánh xe độc lập
|
||
/// Implement InverseKinematics và OdometryEstimator
|
||
/// Sử dụng state machine để quản lý trạng thái
|
||
/// </summary>
|
||
public class DifferentialDrive : IInverseKinematics, IOdometryEstimator, IHostedService, IDisposable
|
||
{
|
||
private readonly PassiveStateMachine<DifferentialDriveState, DifferentialDriveTrigger> _stateMachine;
|
||
private readonly DifferentialDriveConfiguration _config;
|
||
private readonly IDeviceProvider _deviceProvider;
|
||
private readonly ILogger<DifferentialDrive> _logger;
|
||
private readonly IPlcController? _plcController;
|
||
private readonly Lock _lock = new();
|
||
|
||
// Servo devices
|
||
private ICiA402Servo? _leftWheelServo;
|
||
private ICiA402Servo? _rightWheelServo;
|
||
|
||
// Odometry state
|
||
private Pose _currentPose = new();
|
||
private int _lastLeftWheelPosition;
|
||
private int _lastRightWheelPosition;
|
||
private bool _isFirstUpdate = true;
|
||
|
||
// Calculated parameters
|
||
private double _leftWheelRadius;
|
||
private double _rightWheelRadius;
|
||
public double _wheelbase; // Khoảng cách giữa 2 bánh xe
|
||
|
||
// Conversion factors
|
||
private double _leftWheelMetersPerCount; // Mét trên encoder count
|
||
private double _rightWheelMetersPerCount; // Mét trên encoder count
|
||
|
||
private DifferentialDriveState _currentState = DifferentialDriveState.SwitchOnDisabled;
|
||
private bool _disposed = false;
|
||
|
||
/// <summary>
|
||
/// Gets the current state of the DifferentialDrive
|
||
/// </summary>
|
||
public DifferentialDriveState State => _currentState;
|
||
|
||
/// <summary>
|
||
/// Gets whether the drive is in OperationEnabled state (ready to accept velocity commands)
|
||
/// </summary>
|
||
public bool IsOperationEnabled => _currentState == DifferentialDriveState.OperationEnabled;
|
||
|
||
/// <summary>
|
||
/// Gets the current pose (odometry)
|
||
/// </summary>
|
||
public Pose CurrentPose
|
||
{
|
||
get
|
||
{
|
||
lock (_lock)
|
||
{
|
||
return _currentPose;
|
||
}
|
||
}
|
||
}
|
||
|
||
public DifferentialDrive(
|
||
IConfiguration configuration,
|
||
IDeviceProvider deviceProvider,
|
||
ILogger<DifferentialDrive> logger,
|
||
IPlcController? plcController = null)
|
||
{
|
||
_deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
|
||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||
_plcController = plcController;
|
||
|
||
// Load configuration
|
||
var configSection = configuration.GetSection("Motion:DifferentialDrive");
|
||
if (!configSection.Exists())
|
||
{
|
||
throw new InvalidOperationException("Configuration section 'Motion:DifferentialDrive' not found in appsettings.json");
|
||
}
|
||
|
||
_config = new DifferentialDriveConfiguration();
|
||
configSection.Bind(_config);
|
||
|
||
// Validate configuration
|
||
ValidateConfiguration();
|
||
|
||
// Calculate parameters
|
||
CalculateParameters();
|
||
|
||
// Build state machine
|
||
_stateMachine = BuildStateMachine();
|
||
_stateMachine.Start();
|
||
}
|
||
|
||
private void ValidateConfiguration()
|
||
{
|
||
if (string.IsNullOrWhiteSpace(_config.LeftWheel.DeviceId))
|
||
throw new InvalidOperationException("LeftWheel.DeviceId is required");
|
||
|
||
if (string.IsNullOrWhiteSpace(_config.RightWheel.DeviceId))
|
||
throw new InvalidOperationException("RightWheel.DeviceId is required");
|
||
|
||
if (_config.LeftWheel.WheelDiameter <= 0)
|
||
throw new InvalidOperationException("LeftWheel.WheelDiameter must be greater than 0");
|
||
|
||
if (_config.RightWheel.WheelDiameter <= 0)
|
||
throw new InvalidOperationException("RightWheel.WheelDiameter must be greater than 0");
|
||
|
||
if (_config.LeftWheel.PulsesPerRevolution <= 0)
|
||
throw new InvalidOperationException("LeftWheel.PulsesPerRevolution must be greater than 0");
|
||
|
||
if (_config.RightWheel.PulsesPerRevolution <= 0)
|
||
throw new InvalidOperationException("RightWheel.PulsesPerRevolution must be greater than 0");
|
||
}
|
||
|
||
private void CalculateParameters()
|
||
{
|
||
// Calculate wheel radius
|
||
_leftWheelRadius = _config.LeftWheel.WheelDiameter / 2.0;
|
||
_rightWheelRadius = _config.RightWheel.WheelDiameter / 2.0;
|
||
|
||
// Calculate meters per encoder count
|
||
var leftWheelCircumference = Math.PI * _config.LeftWheel.WheelDiameter;
|
||
var rightWheelCircumference = Math.PI * _config.RightWheel.WheelDiameter;
|
||
_leftWheelMetersPerCount = leftWheelCircumference / _config.LeftWheel.PulsesPerRevolution;
|
||
_rightWheelMetersPerCount = rightWheelCircumference / _config.RightWheel.PulsesPerRevolution;
|
||
|
||
// Extract wheel positions
|
||
var leftPos = _config.LeftWheel.Position.Position;
|
||
var rightPos = _config.RightWheel.Position.Position;
|
||
|
||
// Calculate wheelbase (distance between wheel contact points)
|
||
var dx = rightPos.X - leftPos.X;
|
||
var dy = rightPos.Y - leftPos.Y;
|
||
_wheelbase = Math.Sqrt(dx * dx + dy * dy);
|
||
// Console.WriteLine($"rightPos.Y: {rightPos.Y}, leftPos.Y: {leftPos.Y}");
|
||
|
||
_logger.LogInformation(
|
||
"DifferentialDrive parameters calculated: LeftRadius={LeftRadius}m, RightRadius={RightRadius}m, " +
|
||
"Wheelbase={Wheelbase}m, LeftY={LeftY}m, RightY={RightY}m",
|
||
_leftWheelRadius, _rightWheelRadius, _wheelbase, leftPos.Y, rightPos.Y);
|
||
}
|
||
|
||
private PassiveStateMachine<DifferentialDriveState, DifferentialDriveTrigger> BuildStateMachine()
|
||
{
|
||
var builder = new StateMachineDefinitionBuilder<DifferentialDriveState, DifferentialDriveTrigger>();
|
||
|
||
// SwitchOnDisabled state (initial state)
|
||
builder.In(DifferentialDriveState.SwitchOnDisabled)
|
||
.ExecuteOnEntry(() =>
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_currentState = DifferentialDriveState.SwitchOnDisabled;
|
||
}
|
||
_logger.LogInformation("DifferentialDrive state: SwitchOnDisabled");
|
||
})
|
||
.On(DifferentialDriveTrigger.Shutdown)
|
||
.Goto(DifferentialDriveState.ReadyToSwitchOn)
|
||
.Execute(() =>
|
||
{
|
||
_ = Task.Run(InitializeAsync);
|
||
});
|
||
|
||
// NotReadyToSwitchOn state
|
||
builder.In(DifferentialDriveState.NotReadyToSwitchOn)
|
||
.ExecuteOnEntry(() =>
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_currentState = DifferentialDriveState.NotReadyToSwitchOn;
|
||
}
|
||
_logger.LogWarning("DifferentialDrive state: NotReadyToSwitchOn");
|
||
})
|
||
.On(DifferentialDriveTrigger.DisableVoltage)
|
||
.Goto(DifferentialDriveState.SwitchOnDisabled);
|
||
|
||
// ReadyToSwitchOn state
|
||
builder.In(DifferentialDriveState.ReadyToSwitchOn)
|
||
.ExecuteOnEntry(() =>
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_currentState = DifferentialDriveState.ReadyToSwitchOn;
|
||
}
|
||
_logger.LogInformation("DifferentialDrive state: ReadyToSwitchOn");
|
||
})
|
||
.On(DifferentialDriveTrigger.SwitchOn)
|
||
.Goto(DifferentialDriveState.SwitchedOn)
|
||
.Execute(() =>
|
||
{
|
||
_ = Task.Run(SwitchOnAsync);
|
||
})
|
||
.On(DifferentialDriveTrigger.DisableVoltage)
|
||
.Goto(DifferentialDriveState.SwitchOnDisabled)
|
||
.Execute(() =>
|
||
{
|
||
_ = Task.Run(DisableAsync);
|
||
});
|
||
|
||
// SwitchedOn state
|
||
builder.In(DifferentialDriveState.SwitchedOn)
|
||
.ExecuteOnEntry(() =>
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_currentState = DifferentialDriveState.SwitchedOn;
|
||
}
|
||
_logger.LogInformation("DifferentialDrive state: SwitchedOn");
|
||
})
|
||
.On(DifferentialDriveTrigger.EnableOperation)
|
||
.Goto(DifferentialDriveState.OperationEnabled)
|
||
.Execute(() =>
|
||
{
|
||
_ = Task.Run(EnableOperationAsync);
|
||
})
|
||
.On(DifferentialDriveTrigger.DisableOperation)
|
||
.Goto(DifferentialDriveState.ReadyToSwitchOn)
|
||
.On(DifferentialDriveTrigger.QuickStop)
|
||
.Goto(DifferentialDriveState.QuickStopActive)
|
||
.Execute(() =>
|
||
{
|
||
_ = Task.Run(QuickStopAsync);
|
||
})
|
||
.On(DifferentialDriveTrigger.DisableVoltage)
|
||
.Goto(DifferentialDriveState.SwitchOnDisabled)
|
||
.Execute(() =>
|
||
{
|
||
_ = Task.Run(DisableAsync);
|
||
});
|
||
|
||
// OperationEnabled state
|
||
builder.In(DifferentialDriveState.OperationEnabled)
|
||
.ExecuteOnEntry(() =>
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_currentState = DifferentialDriveState.OperationEnabled;
|
||
}
|
||
_logger.LogInformation("DifferentialDrive state: OperationEnabled");
|
||
})
|
||
.On(DifferentialDriveTrigger.DisableOperation)
|
||
.Goto(DifferentialDriveState.SwitchedOn)
|
||
.Execute(() =>
|
||
{
|
||
_ = Task.Run(DisableOperationAsync);
|
||
})
|
||
.On(DifferentialDriveTrigger.QuickStop)
|
||
.Goto(DifferentialDriveState.QuickStopActive)
|
||
.Execute(() =>
|
||
{
|
||
_ = Task.Run(QuickStopAsync);
|
||
})
|
||
.On(DifferentialDriveTrigger.DisableVoltage)
|
||
.Goto(DifferentialDriveState.SwitchOnDisabled)
|
||
.Execute(() =>
|
||
{
|
||
_ = Task.Run(DisableAsync);
|
||
});
|
||
|
||
// QuickStopActive state
|
||
builder.In(DifferentialDriveState.QuickStopActive)
|
||
.ExecuteOnEntry(() =>
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_currentState = DifferentialDriveState.QuickStopActive;
|
||
}
|
||
_logger.LogWarning("DifferentialDrive state: QuickStopActive");
|
||
})
|
||
.On(DifferentialDriveTrigger.DisableVoltage)
|
||
.Goto(DifferentialDriveState.SwitchOnDisabled)
|
||
.Execute(() =>
|
||
{
|
||
_ = Task.Run(DisableAsync);
|
||
});
|
||
|
||
// FaultReactionActive state
|
||
builder.In(DifferentialDriveState.FaultReactionActive)
|
||
.ExecuteOnEntry(() =>
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_currentState = DifferentialDriveState.FaultReactionActive;
|
||
}
|
||
_logger.LogError("DifferentialDrive state: FaultReactionActive");
|
||
})
|
||
.On(DifferentialDriveTrigger.FaultReset)
|
||
.Goto(DifferentialDriveState.SwitchOnDisabled)
|
||
.Execute(() =>
|
||
{
|
||
ResetBothServoFaults();
|
||
_logger.LogInformation("DifferentialDrive fault reset");
|
||
});
|
||
|
||
// Fault state
|
||
builder.In(DifferentialDriveState.Fault)
|
||
.ExecuteOnEntry(() =>
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_currentState = DifferentialDriveState.Fault;
|
||
}
|
||
_logger.LogError("DifferentialDrive state: Fault");
|
||
})
|
||
.On(DifferentialDriveTrigger.FaultReset)
|
||
.Goto(DifferentialDriveState.SwitchOnDisabled)
|
||
.Execute(() =>
|
||
{
|
||
ResetBothServoFaults();
|
||
_logger.LogInformation("DifferentialDrive fault reset");
|
||
});
|
||
|
||
return builder
|
||
.WithInitialState(DifferentialDriveState.SwitchOnDisabled)
|
||
.Build()
|
||
.CreatePassiveStateMachine();
|
||
}
|
||
|
||
private async Task InitializeAsync()
|
||
{
|
||
try
|
||
{
|
||
// Get servo devices
|
||
var leftDevice = _deviceProvider.GetDevice(_config.LeftWheel.DeviceId);
|
||
var rightDevice = _deviceProvider.GetDevice(_config.RightWheel.DeviceId);
|
||
|
||
if (leftDevice is not ICiA402Servo leftServo)
|
||
{
|
||
_logger.LogError("Left wheel device '{DeviceId}' is not an ICiA402Servo", _config.LeftWheel.DeviceId);
|
||
_stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
|
||
return;
|
||
}
|
||
|
||
if (rightDevice is not ICiA402Servo rightServo)
|
||
{
|
||
_logger.LogError("Right wheel device '{DeviceId}' is not an ICiA402Servo", _config.RightWheel.DeviceId);
|
||
_stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
|
||
return;
|
||
}
|
||
|
||
_leftWheelServo = leftServo;
|
||
_rightWheelServo = rightServo;
|
||
|
||
// Subscribe to position changed events
|
||
_leftWheelServo.PositionChanged += OnLeftWheelPositionChanged;
|
||
_rightWheelServo.PositionChanged += OnRightWheelPositionChanged;
|
||
|
||
// Subscribe to statusword changed events to detect faults
|
||
_leftWheelServo.StatuswordChanged += OnLeftWheelStatuswordChanged;
|
||
_rightWheelServo.StatuswordChanged += OnRightWheelStatuswordChanged;
|
||
|
||
// Set operation mode to Profile Velocity
|
||
await _leftWheelServo.SetOperationModeAsync(OperationMode.ProfileVelocity);
|
||
await _rightWheelServo.SetOperationModeAsync(OperationMode.ProfileVelocity);
|
||
|
||
// Set profile velocity to 0 (đảm bảo động cơ không quay)
|
||
await _leftWheelServo.SetProfileVelocityAsync(0);
|
||
await _rightWheelServo.SetProfileVelocityAsync(0);
|
||
|
||
// Set target velocity to 0 (đảm bảo động cơ không quay)
|
||
await _leftWheelServo.SetTargetVelocityAsync(0);
|
||
await _rightWheelServo.SetTargetVelocityAsync(0);
|
||
|
||
// Get initial positions
|
||
_lastLeftWheelPosition = _leftWheelServo.CachedPosition;
|
||
_lastRightWheelPosition = _rightWheelServo.CachedPosition;
|
||
|
||
_logger.LogInformation("DifferentialDrive initialized successfully - servos ready, velocities set to 0");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Failed to initialize DifferentialDrive");
|
||
_stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
|
||
}
|
||
}
|
||
|
||
private async Task DisableAsync()
|
||
{
|
||
try
|
||
{
|
||
// Stop both wheels
|
||
if (_leftWheelServo != null)
|
||
{
|
||
await _leftWheelServo.SetTargetVelocityAsync(0);
|
||
await _leftWheelServo.DisableAsync();
|
||
_leftWheelServo.PositionChanged -= OnLeftWheelPositionChanged;
|
||
_leftWheelServo.StatuswordChanged -= OnLeftWheelStatuswordChanged;
|
||
}
|
||
|
||
if (_rightWheelServo != null)
|
||
{
|
||
await _rightWheelServo.SetTargetVelocityAsync(0);
|
||
await _rightWheelServo.DisableAsync();
|
||
_rightWheelServo.PositionChanged -= OnRightWheelPositionChanged;
|
||
_rightWheelServo.StatuswordChanged -= OnRightWheelStatuswordChanged;
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error disabling DifferentialDrive");
|
||
}
|
||
}
|
||
|
||
private async Task SwitchOnAsync()
|
||
{
|
||
try
|
||
{
|
||
// Switch on both servos (enable voltage, switch on)
|
||
if (_leftWheelServo != null)
|
||
{
|
||
await _leftWheelServo.SwitchOnAsync();
|
||
}
|
||
|
||
if (_rightWheelServo != null)
|
||
{
|
||
await _rightWheelServo.SwitchOnAsync();
|
||
}
|
||
|
||
_logger.LogInformation("DifferentialDrive switched on");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error switching on DifferentialDrive");
|
||
}
|
||
}
|
||
|
||
private async Task EnableOperationAsync()
|
||
{
|
||
try
|
||
{
|
||
// Enable operation on both servos
|
||
if (_leftWheelServo != null)
|
||
{
|
||
await _leftWheelServo.EnableOperationAsync();
|
||
}
|
||
|
||
if (_rightWheelServo != null)
|
||
{
|
||
await _rightWheelServo.EnableOperationAsync();
|
||
}
|
||
|
||
_logger.LogInformation("DifferentialDrive operation enabled");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error enabling operation");
|
||
}
|
||
}
|
||
|
||
private async Task QuickStopAsync()
|
||
{
|
||
try
|
||
{
|
||
// Stop both wheels immediately
|
||
if (_leftWheelServo != null)
|
||
{
|
||
await _leftWheelServo.SetTargetVelocityAsync(0);
|
||
await _leftWheelServo.QuickStopAsync();
|
||
}
|
||
|
||
if (_rightWheelServo != null)
|
||
{
|
||
await _rightWheelServo.SetTargetVelocityAsync(0);
|
||
await _rightWheelServo.QuickStopAsync();
|
||
}
|
||
|
||
_logger.LogWarning("DifferentialDrive quick stop executed");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error executing quick stop");
|
||
}
|
||
}
|
||
|
||
private async Task DisableOperationAsync()
|
||
{
|
||
try
|
||
{
|
||
// Stop both wheels
|
||
if (_leftWheelServo != null)
|
||
{
|
||
await _leftWheelServo.SetTargetVelocityAsync(0);
|
||
await _leftWheelServo.DisableOperationAsync();
|
||
}
|
||
|
||
if (_rightWheelServo != null)
|
||
{
|
||
await _rightWheelServo.SetTargetVelocityAsync(0);
|
||
await _rightWheelServo.DisableOperationAsync();
|
||
}
|
||
|
||
_logger.LogInformation("DifferentialDrive operation disabled");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error disabling operation");
|
||
}
|
||
}
|
||
|
||
/// <summary>Reset fault trên cả hai servo CiA402 (trái/phải) trước khi chuyển drive state — gọi khi FaultReset trigger.</summary>
|
||
private void ResetBothServoFaults()
|
||
{
|
||
try
|
||
{
|
||
var left = _leftWheelServo?.FaultResetAsync(CancellationToken.None);
|
||
var right = _rightWheelServo?.FaultResetAsync(CancellationToken.None);
|
||
if (left != null) left.GetAwaiter().GetResult();
|
||
if (right != null) right.GetAwaiter().GetResult();
|
||
_logger.LogInformation("DifferentialDrive: both wheel servos fault reset sent");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogWarning(ex, "DifferentialDrive: error resetting servo faults");
|
||
}
|
||
}
|
||
|
||
private void OnLeftWheelPositionChanged(object? sender, PositionChangedEventArgs e)
|
||
{
|
||
// Update odometry when either wheel position changes
|
||
UpdateOdometry();
|
||
}
|
||
|
||
private void OnRightWheelPositionChanged(object? sender, PositionChangedEventArgs e)
|
||
{
|
||
// Update odometry when either wheel position changes
|
||
UpdateOdometry();
|
||
}
|
||
|
||
private void OnLeftWheelStatuswordChanged(object? sender, StatuswordChangedEventArgs e)
|
||
{
|
||
if (e.NewState == DriveState.Fault || e.NewState == DriveState.FaultReactionActive)
|
||
{
|
||
_logger.LogError("Left wheel servo entered fault state: {State}", e.NewState);
|
||
_stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
|
||
}
|
||
}
|
||
|
||
private void OnRightWheelStatuswordChanged(object? sender, StatuswordChangedEventArgs e)
|
||
{
|
||
if (e.NewState == DriveState.Fault || e.NewState == DriveState.FaultReactionActive)
|
||
{
|
||
_logger.LogError("Right wheel servo entered fault state: {State}", e.NewState);
|
||
_stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
|
||
}
|
||
}
|
||
|
||
#region IInverseKinematics Implementation
|
||
|
||
/// <summary>
|
||
/// Tính toán vận tốc các bánh xe từ vận tốc robot (Twist)
|
||
/// Sử dụng position thực tế của bánh xe (không phụ thuộc vào tên "left/right")
|
||
///
|
||
/// Công thức kinematics tổng quát cho differential drive:
|
||
/// 1. Tính vận tốc tại contact point: v_contact = v_robot + ω × r_wheel
|
||
/// 2. Bánh xe roll perpendicular to position vector (tangent of circle)
|
||
/// 3. Project v_contact lên roll direction
|
||
/// 4. Apply IsReversed nếu motor đấu ngược
|
||
/// Roll direction = perpendicular to position vector (for differential drive)
|
||
/// </summary>
|
||
public (int leftWheelVelocity, int rightWheelVelocity) CalculateWheelVelocities(Twist twist)
|
||
{
|
||
// Robot velocity
|
||
var vx = twist.Linear.X;
|
||
var vy = twist.Linear.Y;
|
||
var omega = -twist.Angular.Z;
|
||
// var wheelbase = _wheelbase;
|
||
// Console.WriteLine($"vx: {vx}, vy: {vy}, omega: {omega}");
|
||
// Console.WriteLine($"wheelbase: {_wheelbase}");
|
||
|
||
// Wheel positions
|
||
var leftPos = _config.LeftWheel.Position.Position;
|
||
var rightPos = _config.RightWheel.Position.Position;
|
||
|
||
// Bước 1: Tính vận tốc tại contact point của mỗi bánh xe
|
||
// v_contact = v_robot + ω × r
|
||
// Cross product trong 2D: ω × (x, y) = (-ω×y, ω×x)
|
||
var leftContactVx = vx + omega * leftPos.Y;
|
||
var leftContactVy = vy - omega * leftPos.X;
|
||
|
||
var rightContactVx = vx + omega * rightPos.Y;
|
||
var rightContactVy = vy - omega * rightPos.X;
|
||
// Bước 2: Roll direction của bánh xe trong differential drive
|
||
// Roll direction = perpendicular to position vector (tangent of circle)
|
||
// Với position = (x, y), perpendicular = (-y, x) normalized
|
||
// Nhưng cho differential drive đơn giản với wheels at (0, ±Y):
|
||
// Position vector = (0, Y) → perpendicular = (-Y, 0) normalized = (±1, 0)
|
||
// → Tất cả bánh đều roll theo X direction!
|
||
|
||
// Roll direction cho differential drive: luôn theo X direction (forward của robot)
|
||
var leftRollDirX = -1.0;
|
||
var leftRollDirY = 0.0;
|
||
|
||
var rightRollDirX = -1.0;
|
||
var rightRollDirY = 0.0;
|
||
|
||
// Bước 3: Project v_contact lên roll direction (dot product)
|
||
// v_wheel = v_contact · roll_direction
|
||
var leftWheelVelocity = leftContactVx * leftRollDirX + leftContactVy * leftRollDirY;
|
||
var rightWheelVelocity = rightContactVx * rightRollDirX + rightContactVy * rightRollDirY;
|
||
// Console.WriteLine($"leftwheelVelocity (m/s): {leftWheelVelocity}, rightWheelVelocity (m/s): {rightWheelVelocity}");
|
||
|
||
// Bước 4: Apply reversed flag if motor/encoder wired backwards
|
||
if (_config.LeftWheel.IsReversed)
|
||
leftWheelVelocity = -leftWheelVelocity;
|
||
|
||
if (_config.RightWheel.IsReversed)
|
||
rightWheelVelocity = -rightWheelVelocity;
|
||
|
||
// Bước 5: Chuyển đổi từ m/s sang encoder counts/s
|
||
var leftWheelVelocityCounts = (int)(leftWheelVelocity / _leftWheelMetersPerCount);
|
||
var rightWheelVelocityCounts = (int)(rightWheelVelocity / _rightWheelMetersPerCount);
|
||
return (leftWheelVelocityCounts, rightWheelVelocityCounts);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Đặt vận tốc robot (Twist) - sẽ tự động tính toán và điều khiển các bánh xe
|
||
/// </summary>
|
||
public async Task SetVelocityAsync(Twist twist, CancellationToken ct = default)
|
||
{
|
||
if (_currentState != DifferentialDriveState.OperationEnabled)
|
||
{
|
||
if (_currentState == DifferentialDriveState.QuickStopActive ||
|
||
_currentState == DifferentialDriveState.SwitchedOn)
|
||
{
|
||
_logger.LogWarning("DifferentialDrive not in OperationEnabled state ({State}), attempting recovery...", _currentState);
|
||
FaultReset();
|
||
await Task.Delay(200, ct);
|
||
Enable();
|
||
await Task.Delay(300, ct);
|
||
|
||
if (_currentState != DifferentialDriveState.OperationEnabled)
|
||
{
|
||
_logger.LogError("Recovery failed: DifferentialDrive not operational. State: {State}", _currentState);
|
||
return;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
_logger.LogWarning("Cannot set velocity: DifferentialDrive state is {State}", _currentState);
|
||
return;
|
||
}
|
||
}
|
||
|
||
var (leftVel, rightVel) = CalculateWheelVelocities(twist);
|
||
|
||
try
|
||
{
|
||
// Ưu tiên gửi cả hai bánh trong một khung (RPDO) để chúng nhận lệnh đồng thời.
|
||
// Nếu driver/firmware không hỗ trợ thì fallback về ghi SDO từng bánh.
|
||
bool sentTogether = false;
|
||
if (_leftWheelServo is IDualAxisVelocityCommand dualAxis)
|
||
{
|
||
sentTogether = await dualAxis.TrySetBothTargetVelocitiesAsync(leftVel, rightVel, ct);
|
||
}
|
||
|
||
if (!sentTogether)
|
||
{
|
||
if (_leftWheelServo != null)
|
||
{
|
||
await _leftWheelServo.SetTargetVelocityAsync(leftVel, ct);
|
||
}
|
||
|
||
if (_rightWheelServo != null)
|
||
{
|
||
await _rightWheelServo.SetTargetVelocityAsync(rightVel, ct);
|
||
}
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error setting wheel velocities");
|
||
_stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
|
||
}
|
||
|
||
// Truyền hướng di chuyển xuống PLC: tiến M931, lùi M932, không đi thì cả hai off
|
||
var vx = twist.Linear.X;
|
||
const double zeroThreshold = 0.01;
|
||
_plcController?.SetDirectionForwardBackward(vx > zeroThreshold, vx < -zeroThreshold);
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region IOdometryEstimator Implementation
|
||
|
||
/// <summary>
|
||
/// Reset odometry về vị trí ban đầu (0, 0, 0)
|
||
/// </summary>
|
||
public void Reset()
|
||
{
|
||
Reset(new Pose());
|
||
}
|
||
|
||
/// <summary>
|
||
/// Reset odometry về một pose cụ thể
|
||
/// </summary>
|
||
public void Reset(Pose pose)
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_currentPose = pose;
|
||
_isFirstUpdate = true;
|
||
|
||
if (_leftWheelServo != null)
|
||
{
|
||
_lastLeftWheelPosition = _leftWheelServo.CachedPosition;
|
||
}
|
||
|
||
if (_rightWheelServo != null)
|
||
{
|
||
_lastRightWheelPosition = _rightWheelServo.CachedPosition;
|
||
}
|
||
|
||
_logger.LogInformation("Odometry reset to pose: X={X}, Y={Y}, Z={Z}", pose.Position.X, pose.Position.Y, pose.Position.Z);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Cập nhật odometry từ vị trí encoder của các bánh xe
|
||
/// </summary>
|
||
public void Update(int leftWheelPosition, int rightWheelPosition)
|
||
{
|
||
lock (_lock)
|
||
{
|
||
if (_isFirstUpdate)
|
||
{
|
||
_lastLeftWheelPosition = leftWheelPosition;
|
||
_lastRightWheelPosition = rightWheelPosition;
|
||
_isFirstUpdate = false;
|
||
return;
|
||
}
|
||
|
||
// Tính delta position (encoder counts)
|
||
var deltaLeft = leftWheelPosition - _lastLeftWheelPosition;
|
||
var deltaRight = rightWheelPosition - _lastRightWheelPosition;
|
||
|
||
// Apply reversed flag (nếu motor đấu ngược, encoder đếm ngược)
|
||
if (_config.LeftWheel.IsReversed)
|
||
deltaLeft = -deltaLeft;
|
||
|
||
if (_config.RightWheel.IsReversed)
|
||
deltaRight = -deltaRight;
|
||
|
||
// Chuyển đổi sang mét
|
||
var deltaLeftMeters = deltaLeft * _leftWheelMetersPerCount;
|
||
var deltaRightMeters = deltaRight * _rightWheelMetersPerCount;
|
||
|
||
// Tính toán vận tốc trung bình và góc quay
|
||
var averageVelocity = (deltaLeftMeters + deltaRightMeters) / 2.0;
|
||
var deltaTheta = (deltaRightMeters - deltaLeftMeters) / _wheelbase;
|
||
|
||
// Cập nhật pose
|
||
// Lấy orientation hiện tại từ quaternion
|
||
var currentTheta = QuaternionToYaw(_currentPose.Orientation);
|
||
|
||
// Tính toán vị trí mới
|
||
var newX = _currentPose.Position.X + averageVelocity * Math.Cos(currentTheta);
|
||
var newY = _currentPose.Position.Y + averageVelocity * Math.Sin(currentTheta);
|
||
var newTheta = currentTheta + deltaTheta;
|
||
|
||
// Cập nhật pose
|
||
_currentPose = new Pose
|
||
{
|
||
Position = new Point(newX, newY, _currentPose.Position.Z),
|
||
Orientation = YawToQuaternion(newTheta)
|
||
};
|
||
|
||
// Cập nhật last positions
|
||
_lastLeftWheelPosition = leftWheelPosition;
|
||
_lastRightWheelPosition = rightWheelPosition;
|
||
}
|
||
}
|
||
|
||
private void UpdateOdometry()
|
||
{
|
||
if (_leftWheelServo == null || _rightWheelServo == null)
|
||
return;
|
||
|
||
Update(_leftWheelServo.CachedPosition, _rightWheelServo.CachedPosition);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Chuyển đổi quaternion sang yaw angle (radians)
|
||
/// </summary>
|
||
private static double QuaternionToYaw(Quaternion q)
|
||
{
|
||
// Yaw = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))
|
||
var sinYaw = 2.0 * (q.W * q.Z + q.X * q.Y);
|
||
var cosYaw = 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z);
|
||
return Math.Atan2(sinYaw, cosYaw);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Chuyển đổi yaw angle (radians) sang quaternion
|
||
/// </summary>
|
||
private static Quaternion YawToQuaternion(double yaw)
|
||
{
|
||
var halfYaw = yaw / 2.0;
|
||
return new Quaternion(0, 0, Math.Sin(halfYaw), Math.Cos(halfYaw));
|
||
}
|
||
|
||
/// <summary>
|
||
/// Set profile acceleration cho drive system (chuyển đổi từ m/s² sang encoder counts/s²)
|
||
/// </summary>
|
||
/// <param name="acceleration">Gia tốc tăng tốc (m/s²)</param>
|
||
/// <param name="ct">Cancellation token</param>
|
||
public async Task SetAccelerationAsync(double acceleration, CancellationToken ct = default)
|
||
{
|
||
try
|
||
{
|
||
// Chuyển đổi từ m/s² sang encoder counts/s²
|
||
// acceleration_counts = acceleration_ms2 / metersPerCount
|
||
var leftAccelerationCounts = (uint)Math.Abs(acceleration / _leftWheelMetersPerCount);
|
||
var rightAccelerationCounts = (uint)Math.Abs(acceleration / _rightWheelMetersPerCount);
|
||
|
||
if (_leftWheelServo != null)
|
||
{
|
||
await _leftWheelServo.SetProfileAccelerationAsync(leftAccelerationCounts, ct);
|
||
}
|
||
|
||
if (_rightWheelServo != null)
|
||
{
|
||
await _rightWheelServo.SetProfileAccelerationAsync(rightAccelerationCounts, ct);
|
||
}
|
||
|
||
_logger.LogInformation("DifferentialDrive acceleration set to: {Acceleration} m/s² (Left: {LeftCounts}, Right: {RightCounts} counts/s²)",
|
||
acceleration, leftAccelerationCounts, rightAccelerationCounts);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error setting acceleration to {Acceleration} m/s²", acceleration);
|
||
throw;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Set profile deceleration cho drive system (chuyển đổi từ m/s² sang encoder counts/s²)
|
||
/// </summary>
|
||
/// <param name="deceleration">Gia tốc giảm tốc (m/s²)</param>
|
||
/// <param name="ct">Cancellation token</param>
|
||
public async Task SetDecelerationAsync(double deceleration, CancellationToken ct = default)
|
||
{
|
||
try
|
||
{
|
||
// Chuyển đổi từ m/s² sang encoder counts/s²
|
||
var leftDecelerationCounts = (uint)Math.Abs(deceleration / _leftWheelMetersPerCount);
|
||
var rightDecelerationCounts = (uint)Math.Abs(deceleration / _rightWheelMetersPerCount);
|
||
|
||
if (_leftWheelServo != null)
|
||
{
|
||
await _leftWheelServo.SetProfileDecelerationAsync(leftDecelerationCounts, ct);
|
||
}
|
||
|
||
if (_rightWheelServo != null)
|
||
{
|
||
await _rightWheelServo.SetProfileDecelerationAsync(rightDecelerationCounts, ct);
|
||
}
|
||
|
||
_logger.LogInformation("DifferentialDrive deceleration set to: {Deceleration} m/s² (Left: {LeftCounts}, Right: {RightCounts} counts/s²)",
|
||
deceleration, leftDecelerationCounts, rightDecelerationCounts);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error setting deceleration to {Deceleration} m/s²", deceleration);
|
||
throw;
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Public Control Methods
|
||
|
||
/// <summary>
|
||
/// Shutdown command - transition from SwitchOnDisabled to ReadyToSwitchOn
|
||
/// </summary>
|
||
public void Shutdown()
|
||
{
|
||
_stateMachine.Fire(DifferentialDriveTrigger.Shutdown);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Switch on command - transition from ReadyToSwitchOn to SwitchedOn
|
||
/// </summary>
|
||
public void SwitchOn()
|
||
{
|
||
_stateMachine.Fire(DifferentialDriveTrigger.SwitchOn);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Enable operation command - transition from SwitchedOn to OperationEnabled
|
||
/// </summary>
|
||
public void EnableOperation()
|
||
{
|
||
_stateMachine.Fire(DifferentialDriveTrigger.EnableOperation);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Disable operation command - transition from OperationEnabled to SwitchedOn
|
||
/// </summary>
|
||
public void DisableOperation()
|
||
{
|
||
_stateMachine.Fire(DifferentialDriveTrigger.DisableOperation);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Quick stop command - transition to QuickStopActive
|
||
/// </summary>
|
||
public void QuickStop()
|
||
{
|
||
_stateMachine.Fire(DifferentialDriveTrigger.QuickStop);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Disable voltage command - transition to SwitchOnDisabled
|
||
/// </summary>
|
||
public void DisableVoltage()
|
||
{
|
||
_stateMachine.Fire(DifferentialDriveTrigger.DisableVoltage);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Fault reset command - luôn gửi fault reset xuống 2 servo, rồi chuyển state machine (nếu đang Fault/FaultReactionActive).
|
||
/// </summary>
|
||
public void FaultReset()
|
||
{
|
||
ResetBothServoFaults();
|
||
_stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Enable DifferentialDrive - convenience method to go through full sequence
|
||
/// </summary>
|
||
public void Enable()
|
||
{
|
||
// Auto-transition through states if needed
|
||
if (_currentState == DifferentialDriveState.SwitchOnDisabled)
|
||
{
|
||
Shutdown();
|
||
}
|
||
else if (_currentState == DifferentialDriveState.ReadyToSwitchOn)
|
||
{
|
||
SwitchOn();
|
||
}
|
||
else if (_currentState == DifferentialDriveState.SwitchedOn)
|
||
{
|
||
EnableOperation();
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Enable 2 động cơ giống enable bằng tay trên device: gửi lệnh trực tiếp xuống từng servo, await từng bước.
|
||
/// </summary>
|
||
public async Task EnableAsync(CancellationToken cancellationToken = default)
|
||
{
|
||
if (_leftWheelServo == null || _rightWheelServo == null)
|
||
{
|
||
_logger.LogWarning("DifferentialDrive.EnableAsync: servo chưa có, bỏ qua");
|
||
return;
|
||
}
|
||
try
|
||
{
|
||
await _leftWheelServo.SetOperationModeAsync(OperationMode.ProfileVelocity, cancellationToken);
|
||
await _rightWheelServo.SetOperationModeAsync(OperationMode.ProfileVelocity, cancellationToken);
|
||
await _leftWheelServo.SetProfileVelocityAsync(0, cancellationToken);
|
||
await _rightWheelServo.SetProfileVelocityAsync(0, cancellationToken);
|
||
await _leftWheelServo.SetTargetVelocityAsync(0, cancellationToken);
|
||
await _rightWheelServo.SetTargetVelocityAsync(0, cancellationToken);
|
||
await _leftWheelServo.SwitchOnAsync(cancellationToken);
|
||
await _rightWheelServo.SwitchOnAsync(cancellationToken);
|
||
await _leftWheelServo.EnableOperationAsync(cancellationToken);
|
||
await _rightWheelServo.EnableOperationAsync(cancellationToken);
|
||
lock (_lock)
|
||
{
|
||
_currentState = DifferentialDriveState.OperationEnabled;
|
||
}
|
||
// Đồng bộ state machine với trạng thái thực để device/điều khiển hoạt động đúng
|
||
_stateMachine.Fire(DifferentialDriveTrigger.Shutdown);
|
||
_stateMachine.Fire(DifferentialDriveTrigger.SwitchOn);
|
||
_stateMachine.Fire(DifferentialDriveTrigger.EnableOperation);
|
||
_logger.LogInformation("DifferentialDrive: 2 động cơ đã enable (EnableAsync), state machine đã sync");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogWarning(ex, "DifferentialDrive.EnableAsync: lỗi khi enable động cơ");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Disable DifferentialDrive - convenience method
|
||
/// </summary>
|
||
public void Disable()
|
||
{
|
||
if (_currentState == DifferentialDriveState.OperationEnabled)
|
||
{
|
||
DisableOperation();
|
||
}
|
||
else if (_currentState == DifferentialDriveState.SwitchedOn ||
|
||
_currentState == DifferentialDriveState.ReadyToSwitchOn)
|
||
{
|
||
DisableVoltage();
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Set operation mode cho cả hai wheel servos
|
||
/// </summary>
|
||
/// <param name="mode">Operation mode to set</param>
|
||
/// <param name="ct">Cancellation token</param>
|
||
public async Task SetOperationModeAsync(OperationMode mode, CancellationToken ct = default)
|
||
{
|
||
try
|
||
{
|
||
if (_leftWheelServo != null)
|
||
{
|
||
await _leftWheelServo.SetOperationModeAsync(mode, ct);
|
||
}
|
||
|
||
if (_rightWheelServo != null)
|
||
{
|
||
await _rightWheelServo.SetOperationModeAsync(mode, ct);
|
||
}
|
||
|
||
_logger.LogInformation("DifferentialDrive operation mode set to: {Mode}", mode);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error setting operation mode to {Mode}", mode);
|
||
throw;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Get current operation mode của left wheel servo (giả sử cả 2 bánh có cùng mode)
|
||
/// </summary>
|
||
/// <param name="ct">Cancellation token</param>
|
||
public async Task<OperationMode> GetOperationModeAsync(CancellationToken ct = default)
|
||
{
|
||
try
|
||
{
|
||
if (_leftWheelServo != null)
|
||
{
|
||
return await _leftWheelServo.GetOperationModeAsync(ct);
|
||
}
|
||
|
||
_logger.LogWarning("Cannot get operation mode: Left wheel servo not available");
|
||
return OperationMode.NoMode;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error getting operation mode");
|
||
return OperationMode.NoMode;
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region IHostedService
|
||
|
||
/// <summary>
|
||
/// Start DifferentialDrive service — init chạy nền để không chặn Kestrel/web UI lúc startup.
|
||
/// </summary>
|
||
public Task StartAsync(CancellationToken cancellationToken)
|
||
{
|
||
_logger.LogInformation("Starting DifferentialDrive service (non-blocking)...");
|
||
_ = Task.Run(() => InitializeWhenDevicesReadyAsync(cancellationToken), cancellationToken);
|
||
return Task.CompletedTask;
|
||
}
|
||
|
||
private async Task InitializeWhenDevicesReadyAsync(CancellationToken cancellationToken)
|
||
{
|
||
try
|
||
{
|
||
_logger.LogInformation("Waiting for all devices to be connected...");
|
||
var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
|
||
|
||
if (!connected)
|
||
{
|
||
_logger.LogWarning("Timeout waiting for devices to connect. DifferentialDrive will not be initialized.");
|
||
return;
|
||
}
|
||
|
||
_logger.LogInformation("All devices connected. Initializing DifferentialDrive...");
|
||
Shutdown();
|
||
}
|
||
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
||
{
|
||
_logger.LogInformation("DifferentialDrive initialization cancelled");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error starting DifferentialDrive service");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Stop DifferentialDrive service
|
||
/// </summary>
|
||
public async Task StopAsync(CancellationToken cancellationToken)
|
||
{
|
||
_logger.LogInformation("Stopping DifferentialDrive service...");
|
||
|
||
try
|
||
{
|
||
await DisableOperationAsync();
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error stopping DifferentialDrive service");
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region IDisposable
|
||
|
||
public void Dispose()
|
||
{
|
||
if (_disposed)
|
||
return;
|
||
|
||
_disposed = true;
|
||
|
||
try
|
||
{
|
||
Disable();
|
||
_stateMachine.Stop();
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger.LogError(ex, "Error disposing DifferentialDrive");
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
}
|
||
|