Initial commit

2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Devices/ICiA402Servo.cs
+++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Devices/ICiA402Servo.cs
@@ -0,0 +1,329 @@
+using RobotNet10.CANOpen.CiA402;
+using RobotNet10.CANOpen.CiA402.Enums;
+using RobotNet10.CANOpen.CiA402.Models;
+
+namespace RobotNet10.RobotApp.Devices;
+
+/// <summary>
+/// Interface cho CiA402 Servo - cung cấp tất cả chức năng điều khiển động cơ servo theo chuẩn CiA402
+/// Tập trung vào phần điều khiển động cơ, không bao gồm các chức năng liên quan đến CAN/PDO
+/// </summary>
+public interface ICiA402Servo
+{
+    // Cached values (thread-safe, read-only)
+    /// <summary>
+    /// Statusword hiện tại (real-time, thread-safe)
+    /// </summary>
+    Statusword CachedStatusword { get; }
+    
+    /// <summary>
+    /// Actual position hiện tại (thread-safe)
+    /// </summary>
+    int CachedPosition { get; }
+    
+    /// <summary>
+    /// Actual velocity hiện tại (thread-safe)
+    /// </summary>
+    int CachedVelocity { get; }
+    
+    /// <summary>
+    /// Actual torque hiện tại (thread-safe)
+    /// </summary>
+    short CachedTorque { get; }
+    
+    // Events
+    /// <summary>
+    /// Event được kích hoạt khi Statusword thay đổi
+    /// </summary>
+    event EventHandler<StatuswordChangedEventArgs>? StatuswordChanged;
+    
+    /// <summary>
+    /// Event được kích hoạt khi Position thay đổi
+    /// </summary>
+    event EventHandler<PositionChangedEventArgs>? PositionChanged;
+    
+    /// <summary>
+    /// Event được kích hoạt khi Velocity thay đổi
+    /// </summary>
+    event EventHandler<VelocityChangedEventArgs>? VelocityChanged;
+    
+    // Statusword & Controlword
+    /// <summary>
+    /// Get statusword hiện tại
+    /// </summary>
+    Task<Statusword> GetStatuswordAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Set controlword để điều khiển motor
+    /// </summary>
+    Task SetControlwordAsync(Controlword controlword, CancellationToken ct = default);
+    
+    /// <summary>
+    /// Get drive state từ statusword
+    /// </summary>
+    Task<DriveState> GetStateAsync(CancellationToken ct = default);
+    
+    // Operation Mode
+    /// <summary>
+    /// Set operation mode (Profile Position, Profile Velocity, Profile Torque, etc.)
+    /// </summary>
+    Task SetOperationModeAsync(OperationMode mode, CancellationToken ct = default);
+    
+    /// <summary>
+    /// Get operation mode hiện tại
+    /// </summary>
+    Task<OperationMode> GetOperationModeAsync(CancellationToken ct = default);
+    
+    // State Machine Control
+    /// <summary>
+    /// Reset fault trên motor
+    /// </summary>
+    Task FaultResetAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Shutdown command
+    /// </summary>
+    Task ShutdownAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Switch on command
+    /// </summary>
+    Task SwitchOnAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Enable operation command
+    /// </summary>
+    Task EnableOperationAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Disable operation command
+    /// </summary>
+    Task DisableOperationAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Quick stop command
+    /// </summary>
+    Task QuickStopAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Enable motor với automatic state transitions
+    /// </summary>
+    Task EnableAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Disable motor
+    /// </summary>
+    Task DisableAsync(CancellationToken ct = default);
+    
+    // Position Control
+    /// <summary>
+    /// Get actual position hiện tại
+    /// </summary>
+    Task<int> GetActualPositionAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Set target position
+    /// </summary>
+    Task SetTargetPositionAsync(int position, CancellationToken ct = default);
+    
+    /// <summary>
+    /// Set profile speed
+    /// </summary>
+    Task SetProfileSpeedAsync(uint velocity, CancellationToken ct = default);
+    
+    /// <summary>
+    /// Set profile velocity
+    /// </summary>
+    Task SetProfileVelocityAsync(uint velocity, CancellationToken ct = default);
+    
+    /// <summary>
+    /// Set profile acceleration
+    /// </summary>
+    Task SetProfileAccelerationAsync(uint acceleration, CancellationToken ct = default);
+    
+    /// <summary>
+    /// Set profile deceleration
+    /// </summary>
+    Task SetProfileDecelerationAsync(uint deceleration, CancellationToken ct = default);
+    
+    /// <summary>
+    /// Get profile speed (0x6081) từ drive
+    /// </summary>
+    Task<uint> GetProfileSpeedAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Get profile acceleration (0x6083) từ drive
+    /// </summary>
+    Task<uint> GetProfileAccelerationAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Get profile deceleration (0x6084) từ drive
+    /// </summary>
+    Task<uint> GetProfileDecelerationAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Start position move (set new setpoint bit)
+    /// </summary>
+    Task StartPositionMoveAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Move to position với Profile Position mode
+    /// </summary>
+    Task MoveToPositionAsync(int position, uint velocity = 1000, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default);
+    
+    // Velocity Control
+    /// <summary>
+    /// Get actual velocity hiện tại
+    /// </summary>
+    Task<int> GetActualVelocityAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Set target velocity
+    /// </summary>
+    Task SetTargetVelocityAsync(int velocity, CancellationToken ct = default);
+    
+    /// <summary>
+    /// Target velocity với Profile Velocity mode
+    /// </summary>
+    Task TargetVelocityAsync(int targetVelocity, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default);
+    
+    /// <summary>
+    /// Profile velocity với Profile Velocity mode
+    /// </summary>
+    Task ProfileVelocityAsync(int targetVelocity, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default);
+    
+    // Torque Control
+    /// <summary>
+    /// Get actual torque hiện tại
+    /// </summary>
+    Task<short> GetActualTorqueAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Set target torque
+    /// </summary>
+    Task SetTargetTorqueAsync(short torque, CancellationToken ct = default);
+    
+    /// <summary>
+    /// Run torque với Profile Torque mode
+    /// </summary>
+    Task RunTorqueAsync(short torque, CancellationToken ct = default);
+    
+    // Homing
+    /// <summary>
+    /// Set homing method
+    /// </summary>
+    Task SetHomingMethodAsync(byte method, CancellationToken ct = default);
+
+    /// <summary>
+    /// Set homing speed (speed during search for switch, sub-index 1 of 0x6099).
+    /// </summary>
+    Task SetHomingSpeedAsync(int speed, CancellationToken ct = default);
+
+    /// <summary>
+    /// Set homing offset (0x607C). Offset applied after homing is complete.
+    /// </summary>
+    Task SetHomingOffsetAsync(int offset, CancellationToken ct = default);
+
+    /// <summary>
+    /// Đọc lại homing method từ drive (0x6098) để kiểm tra đã ghi xuống chưa.
+    /// </summary>
+    Task<byte> GetHomingMethodAsync(CancellationToken ct = default);
+
+    /// <summary>
+    /// Đọc lại homing speed từ drive (0x6099 sub-index 1) để kiểm tra đã ghi xuống chưa.
+    /// </summary>
+    Task<int> GetHomingSpeedAsync(CancellationToken ct = default);
+
+    /// <summary>
+    /// Đọc lại homing offset từ drive (0x607C) để kiểm tra đã ghi xuống chưa.
+    /// </summary>
+    Task<int> GetHomingOffsetAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Start homing procedure
+    /// </summary>
+    Task StartHomingAsync(byte method, int speed, CancellationToken ct = default);
+    
+    // Target Position Checking
+    /// <summary>
+    /// Kiểm tra xem motor đã đến vị trí target chưa
+    /// </summary>
+    /// <param name="tolerance">Tolerance cho position comparison (encoder counts). Chỉ dùng nếu không có TargetReached bit.</param>
+    /// <param name="useStatusword">True để dùng Statusword.TargetReached bit (recommended), False để so sánh position</param>
+    bool IsAtTarget(int tolerance = 100, bool useStatusword = true);
+    
+    /// <summary>
+    /// Đợi cho đến khi motor đến vị trí target
+    /// </summary>
+    Task WaitUntilAtTargetAsync(int tolerance = 100, bool useStatusword = true, int checkIntervalMs = 10, CancellationToken ct = default);
+    
+    // Error and Status Information
+    /// <summary>
+    /// Kiểm tra xem motor có đang ở trạng thái Fault không
+    /// </summary>
+    Task<bool> IsInFaultStateAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Đọc Error Register từ device
+    /// </summary>
+    Task<byte> GetErrorRegisterAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Đọc Pre-defined Error Field từ device (danh sách các error codes gần đây)
+    /// </summary>
+    Task<ushort[]> GetErrorHistoryAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Đọc error code mới nhất từ Error History
+    /// </summary>
+    Task<ushort> GetLatestErrorCodeAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Reset fault trên motor (nếu motor đang ở trạng thái Fault)
+    /// </summary>
+    Task<bool> TryFaultResetAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Kiểm tra xem motor có đang enabled (Operation Enabled state) không
+    /// </summary>
+    Task<bool> IsEnabledAsync(CancellationToken ct = default);
+    
+    /// <summary>
+    /// Kiểm tra xem motor có đang ready (Ready to Switch On hoặc Switched On) không
+    /// </summary>
+    Task<bool> IsReadyAsync(CancellationToken ct = default);
+}
+
+#region Event Args
+
+/// <summary>
+/// Event args cho StatuswordChanged event
+/// </summary>
+public class StatuswordChangedEventArgs(Statusword statusword, DriveState oldState, DriveState newState) : EventArgs
+{
+    public Statusword Statusword { get; } = statusword;
+    public DriveState OldState { get; } = oldState;
+    public DriveState NewState { get; } = newState;
+}
+
+/// <summary>
+/// Event args cho PositionChanged event
+/// </summary>
+public class PositionChangedEventArgs(int position, int oldPosition) : EventArgs
+{
+    public int Position { get; } = position;
+    public int OldPosition { get; } = oldPosition;
+    public int Delta => Position - OldPosition;
+}
+
+/// <summary>
+/// Event args cho VelocityChanged event
+/// </summary>
+public class VelocityChangedEventArgs(int velocity, int oldVelocity) : EventArgs
+{
+    public int Velocity { get; } = velocity;
+    public int OldVelocity { get; } = oldVelocity;
+}
+
+#endregion
+